diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 5ffe0a2..04929b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { public static double clawClose = 1.00; - public static double clawOpen = 0.25; + public static double clawOpen = 0.0625; - public static double armFloor = 0.5; - public static double armPark = 0.125; - public static double armBucket = 0.175; + public static double armFloor = 0.375; + public static double armPark = 0.0; + public static double armBucket = 0.15; public static double wristFloor = 0.625; public static double wristBucket = 0.215; @@ -17,5 +17,5 @@ public class RobotConstants { public static int liftToFloatPos = 150; public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3850; - public static double liftPower = .45; + public static double liftPower = .375; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java index e43e73c..3503b14 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; import org.firstinspires.ftc.teamcode.util.action.Actions; import org.firstinspires.ftc.teamcode.util.action.SequentialAction; +import org.firstinspires.ftc.teamcode.util.action.SleepAction; public class DevTeleopRunMode { @@ -33,16 +34,18 @@ public class DevTeleopRunMode { public Gamepad previousGP1; public Gamepad currentGP2; public Gamepad previousGP2; + private Telemetry telemetry; public FieldStates fieldStates; public DevTeleopRunMode(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { - this.motors = new MotorsSubsystem(hardwareMap, telemetry); + this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55); this.claw = new ClawSubsystem(hardwareMap, telemetry); this.arm = new ArmSubsystem(hardwareMap, telemetry); this.wrist = new WristSubsystem(hardwareMap, telemetry); this.lift = new LiftSubsystem(hardwareMap, telemetry); this.GP1 = gp1; this.GP2 = gp2; + this.telemetry = telemetry; this.currentGP1 = new Gamepad(); this.currentGP2 = new Gamepad(); this.previousGP1 = new Gamepad(); @@ -64,11 +67,19 @@ public class DevTeleopRunMode { this.previousGP2.copy(currentGP2); this.currentGP2.copy(this.GP2); this.theDrop(); - this.theLift(); this.thePickup(); this.theTravel(); this.theLowBucketScore(); this.theHighBucketScore(); + this.toHold(); + this.motors.calculateTrajectory(this.GP1); + this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); + this.telemetry.addData("Claw State", this.claw.getState()); + this.telemetry.addData("Claw Position", this.claw.getPosition()); + this.telemetry.addData("Wrist State", this.wrist.getState()); + this.telemetry.addData("Arm State", this.arm.getState()); + this.telemetry.addData("Lift State", this.lift.getState()); + this.telemetry.addData("Lift Position", this.lift.getPosition()); } /* @@ -77,11 +88,13 @@ public class DevTeleopRunMode { Action: On button press, Arm hovers the floor with wrist parallel to arm */ public void theDrop() { - if (this.currentGP1.a && !this.previousGP1.a) { - if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) { + if (this.currentGP1.dpad_down && !this.previousGP1.dpad_down) { + if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD && + this.lift.getPosition() < 40) { Actions.runBlocking(new SequentialAction( - this.arm.toFloorPosition, - this.wrist.toFloorPosition + this.wrist.toFloorPosition, + new SleepAction(.75), + this.arm.toFloorPosition )); } } @@ -99,16 +112,16 @@ public class DevTeleopRunMode { } /* - Controller: 1 - Button: B - Action: On button press, lift arm and prepare wrist for bucket - */ - public void theLift() { - if (this.currentGP1.b && !this.previousGP1.b) { + Controller: 1 + Button: Right Bumper + Action: On button press, open and closes claw + */ + public void toHold() { + if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) { if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) { Actions.runBlocking(new SequentialAction( - this.arm.toParkPosition, - this.wrist.toBucketPosition + arm.toParkPosition, + wrist.toFloorPosition )); } } @@ -121,7 +134,7 @@ public class DevTeleopRunMode { arm to bucket position, wrist to bucket position */ public void theLowBucketScore() { - if (this.currentGP2.y && !this.previousGP2.y) { + if (this.currentGP1.a && !this.previousGP1.a) { fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); Actions.runBlocking(new SequentialAction( lift.toLowBucket, @@ -138,7 +151,7 @@ public class DevTeleopRunMode { arm to bucket position, wrist to bucket position */ public void theHighBucketScore() { - if (this.currentGP2.a && !this.previousGP2.a) { + if (this.currentGP1.b && !this.previousGP1.b) { fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); Actions.runBlocking(new SequentialAction( lift.toHighBucket, @@ -155,7 +168,7 @@ public class DevTeleopRunMode { arm to bucket position, wrist to floor position */ public void theTravel(){ - if (this.currentGP2.dpad_down && !this.previousGP2.dpad_down){ + if (this.currentGP1.dpad_right && !this.previousGP1.dpad_right){ if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.BUCKET) { Actions.runBlocking(new SequentialAction( lift.toFloor, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java index 00dd17e..bee167c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -39,7 +39,6 @@ public class ArmSubsystem { this.arm.setPosition(armBucket); } this.state = armState; - this.telemetry.addData("Arm State", this.getState()); } public void toFloorPosition() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java index 2e66491..52acc57 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java @@ -35,7 +35,6 @@ public class ClawSubsystem { claw.setPosition(clawOpen); } this.state = clawState; - this.telemetry.addData("Claw State", this.getState()); } public ClawState getState() { @@ -66,4 +65,8 @@ public class ClawSubsystem { Actions.runBlocking(closeClaw); } + public double getPosition() { + return this.claw.getPosition(); + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java index 5cbe29d..2b20c7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -38,12 +38,11 @@ public class LiftSubsystem { public void setTarget(int b) { lift.setTargetPosition(b); lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - this.telemetry.addData("Lift State", this.getState()); } public void switchState() { if (this.liftState == LiftState.FLOOR) { - this.toFloor(); + this.toFloat(); } else if (this.liftState == LiftState.FLOAT) { this.toLowBucket(); } else if (this.liftState == LiftState.LOW_BUCKET) { @@ -54,23 +53,23 @@ public class LiftSubsystem { } public void toFloor() { - this.setTarget(liftToFloorPos); this.setState(LiftState.FLOOR); + this.setTarget(liftToFloorPos); } public void toFloat() { - this.setTarget(liftToFloatPos); this.setState(LiftState.FLOAT); + this.setTarget(liftToFloatPos); } public void toLowBucket() { - this.setTarget(liftToLowBucketPos); this.setState(LiftState.LOW_BUCKET); + this.setTarget(liftToLowBucketPos); } public void toHighBucket() { - this.setTarget(liftToHighBucketPos); this.setState(LiftState.HIGH_BUCKET); + this.setTarget(liftToHighBucketPos); } public void init() { @@ -83,7 +82,7 @@ public class LiftSubsystem { this.liftState = liftState; } - private LiftState getState() { return this.liftState; } + public LiftState getState() { return this.liftState; } public int getPosition() { return lift.getCurrentPosition(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java index a5637db..cb89071 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java @@ -35,7 +35,6 @@ public class WristSubsystem { wrist.setPosition(wristBucket); } this.state = wristState; - this.telemetry.addData("Wrist State", this.getState()); } public void toFloorPosition() {