diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CONTROLS.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CONTROLS.md new file mode 100644 index 0000000..a4dfb4e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CONTROLS.md @@ -0,0 +1,12 @@ +# Controller 1 + +## Motor Controls + +- Left Joystick + - Forward & Backwards +- Right Joystick + - Strafe & Turning + +## Arm Controls + +- \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java index cf22479..9aab7d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java @@ -31,10 +31,10 @@ public class DevTeleOpRemix extends OpMode { @Override public void init() { - claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); - arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); - wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); - lift = new LiftSubsystem(hardwareMap); + claw = new ClawSubsystem(hardwareMap, telemetry); + arm = new ArmSubsystem(hardwareMap, telemetry); + wrist = new WristSubsystem(hardwareMap, telemetry); + lift = new LiftSubsystem(hardwareMap, telemetry); motors = new MotorsSubsystem(hardwareMap, telemetry, power); claw.init(); @@ -51,8 +51,8 @@ public class DevTeleOpRemix extends OpMode { public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.a && !previousGamepad1.a) { - wrist.floorWrist(); - arm.engageArm(); + wrist.toFloorPosition(); + arm.toFloorPosition(); } } @@ -64,15 +64,15 @@ public class DevTeleOpRemix extends OpMode { public void theLift(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.b && !previousGamepad1.b) { - arm.parkArm(); - wrist.bucketWrist(); + arm.toParkPosition(); + wrist.toBucketPosition(); } } public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { if (currentGamepad1.y && !previousGamepad1.y) { lift.toLowBucket(); - wrist.bucketWrist(); + wrist.toBucketPosition(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java index 21002ce..73a3805 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java @@ -36,11 +36,11 @@ public class DevTeleOpRemixDeux extends OpMode { follower = new Follower(hardwareMap); - claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); - arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); - wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + claw = new ClawSubsystem(hardwareMap, telemetry); + arm = new ArmSubsystem(hardwareMap, telemetry); + wrist = new WristSubsystem(hardwareMap, telemetry); motors = new MotorsSubsystem(hardwareMap, telemetry); - lift = new LiftSubsystem(hardwareMap); + lift = new LiftSubsystem(hardwareMap, telemetry); claw.init(); arm.init(); @@ -60,8 +60,8 @@ public class DevTeleOpRemixDeux extends OpMode { public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.a && !previousGamepad1.a) { - wrist.floorWrist(); - arm.engageArm(); + wrist.toFloorPosition(); + arm.toFloorPosition(); } } @@ -73,15 +73,15 @@ public class DevTeleOpRemixDeux extends OpMode { public void theLift(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.b && !previousGamepad1.b) { - arm.parkArm(); - wrist.bucketWrist(); + arm.toParkPosition(); + wrist.toBucketPosition(); } } public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { if (currentGamepad1.y && !previousGamepad1.y) { lift.toLowBucket(); - wrist.bucketWrist(); + wrist.toBucketPosition(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index 8288f4e..78aba15 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -1,180 +1,24 @@ package org.firstinspires.ftc.teamcode; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; - -import android.graphics.Point; - import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.Gamepad; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunMode; @TeleOp(name = "Dev Teleop", group = "Debug") public class DevTeleop extends OpMode { - public ClawSubsystem claw; - public ArmSubsystem arm; - public WristSubsystem wrist; - public LiftSubsystem lift; - public Gamepad currentGamepad1; - public Gamepad previousGamepad1; - public Gamepad currentGamepad2; - public Gamepad previousGamepad2; - public DcMotor frontLeftMotor; - public DcMotor backLeftMotor; - public DcMotor frontRightMotor; - public DcMotor backRightMotor; + public DevTeleopRunMode runMode; - private double MAX_POWER = .6; @Override public void init() { - claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); - arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); - wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); - lift = new LiftSubsystem(hardwareMap); - claw.init(); - arm.init(); - wrist.init(); - lift.init(); - - frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR); - backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR); - frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); - backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); - - frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION); - backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION); - frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); - backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION); - - currentGamepad1 = new Gamepad(); - previousGamepad1 = new Gamepad(); - currentGamepad2 = new Gamepad(); - previousGamepad2 = new Gamepad(); + this.runMode = new DevTeleopRunMode(hardwareMap, telemetry, gamepad1, gamepad2); + this.runMode.init(); } - public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { - - if (currentGamepad1.a && !previousGamepad1.a) { - wrist.floorWrist(); - arm.engageArm(); - } - - } - public void thePickup(ClawSubsystem claw) { - - if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { - claw.switchState(); - } - - } - - public void theLift(ArmSubsystem arm, WristSubsystem wrist) { - - if (currentGamepad1.b && !previousGamepad1.b) { - arm.parkArm(); - wrist.bucketWrist(); - } - - } - - public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { - if (currentGamepad2.y && !previousGamepad2.y) { - lift.toLowBucket(); - arm.bucketArm(); - wrist.bucketWrist(); - } - } - public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { - - if (currentGamepad2.a && !previousGamepad2.a) { - lift.toHighBucket(); - arm.bucketArm(); - wrist.bucketWrist(); - } - } - - public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){ - if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down){ - lift.toFloor(); - arm.bucketArm(); - wrist.floorWrist(); - } - } @Override public void loop() { - previousGamepad1.copy(currentGamepad1); - currentGamepad1.copy(gamepad1); - previousGamepad2.copy(currentGamepad2); - currentGamepad2.copy(gamepad2); - - - theDrop(arm, wrist); - thePickup(claw); - theLift(arm, wrist); - theLowBucketScore(lift, wrist, arm); - theHighBucketScore(lift, wrist, arm); - theTravel(lift, arm, wrist); - - double max; - - // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. - double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value - double lateral = gamepad1.left_stick_x; - double yaw = gamepad1.right_stick_x; - - // Combine the joystick requests for each axis-motion to determine each wheel's power. - // Set up a variable for each drive wheel to save the power level for telemetry. - double leftFrontPower = axial + lateral + yaw; - double rightFrontPower = axial - lateral - yaw; - double leftBackPower = axial - lateral + yaw; - double rightBackPower = axial + lateral - yaw; - - // Normalize the values so no wheel power exceeds 100% - // This ensures that the robot maintains the desired motion. - max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); - max = Math.max(max, Math.abs(leftBackPower)); - max = Math.max(max, Math.abs(rightBackPower)); - - if (max > 1.0) { - leftFrontPower /= max; - rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; - } - - - // Send calculated power to wheels - frontLeftMotor.setPower(leftFrontPower * MAX_POWER); - frontRightMotor.setPower(rightFrontPower * MAX_POWER); - backLeftMotor.setPower(leftBackPower * MAX_POWER); - backRightMotor.setPower(rightBackPower * MAX_POWER); - - // Show the elapsed game time and wheel power. - telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); - telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); - telemetry.addData("Current Lift Position", lift.getPosition()); + this.runMode.update(); telemetry.update(); - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java index 778c4c8..67007ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java @@ -10,6 +10,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; @@ -225,6 +226,14 @@ public class BluebAutoV1 extends OpMode { ) .setConstantHeadingInterpolation(Math.toRadians(90)).build(); + + follower.holdPoint( + new BezierPoint( + new Point(13.821, 134.839, Point.CARTESIAN) + ), + 90 + ); + follower.followPath(path); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.update(); @@ -239,7 +248,7 @@ public class BluebAutoV1 extends OpMode { public void loop() { follower.update(); if (follower.atParametricEnd()) { - follower.followPath(path); + follower.followPath(path, true); } follower.telemetryDebug(telemetryA); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java index 048f264..79a18c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java @@ -48,7 +48,7 @@ public class ArmTest extends LinearOpMode { /* * Instantiate Arm */ - ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); + ArmSubsystem arm = new ArmSubsystem(hardwareMap, telemetry); /* * Instantiate gamepad state holders @@ -67,11 +67,11 @@ public class ArmTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.circle && !previousGamepad1.circle) { - arm.parkArm(); + arm.toParkPosition(); } if (currentGamepad1.square && !previousGamepad1.square) { - arm.engageArm(); + arm.toBucketPosition(); } if (currentGamepad1.cross && !previousGamepad1.cross) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java index 278df77..a16f1ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java @@ -48,7 +48,7 @@ public class ClawTest extends LinearOpMode { /* * Instantiate Claw */ - ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN); + ClawSubsystem claw = new ClawSubsystem(hardwareMap, telemetry); /* * Instantiate gamepad state holders diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java index 88267d3..3867daa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java @@ -54,7 +54,7 @@ public class LiftTest extends LinearOpMode { /* * Instantiate Lift */ - LiftSubsystem lift = new LiftSubsystem(hardwareMap); + LiftSubsystem lift = new LiftSubsystem(hardwareMap, telemetry); /* * Instantiate gamepad state holders diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java index 3ead2d6..028cd81 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java @@ -48,7 +48,7 @@ public class WristTest extends LinearOpMode { /* * Instantiate Wrist */ - WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + WristSubsystem wrist = new WristSubsystem(hardwareMap, telemetry); /* * Instantiate gamepad state holders @@ -67,11 +67,11 @@ public class WristTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.square && !previousGamepad1.square) { - wrist.bucketWrist(); + wrist.toBucketPosition(); } if (currentGamepad1.circle && !previousGamepad1.circle) { - wrist.floorWrist(); + wrist.toFloorPosition(); } if (currentGamepad1.cross && !previousGamepad1.cross) { 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 d56cf6b..5ffe0a2 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 @@ -7,7 +7,7 @@ public class RobotConstants { public static double clawClose = 1.00; public static double clawOpen = 0.25; - public static double armEngage = 0.5; + public static double armFloor = 0.5; public static double armPark = 0.125; public static double armBucket = 0.175; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java index 6fd3fc2..d4e0460 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java @@ -36,9 +36,9 @@ public class Auto { public int caseState = 1; public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) { - claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); - arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); - wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + claw = new ClawSubsystem(hardwareMap, telemetry); + arm = new ArmSubsystem(hardwareMap, telemetry); + wrist = new WristSubsystem(hardwareMap, telemetry); this.follower = follower; this.telemetry = telemetry; @@ -77,13 +77,13 @@ public class Auto { break; case 2: if (clawTimer.getElapsedTimeSeconds() > 2) { - arm.engageArm(); + arm.toFloorPosition(); caseState = 3; } break; case 3: if (armTimer.getElapsedTimeSeconds() > 4) { - wrist.floorWrist(); + wrist.toFloorPosition(); caseState = 4; } break; @@ -95,8 +95,8 @@ public class Auto { break; case 5: if (armTimer.getElapsedTimeSeconds() > 8) { - arm.bucketArm(); - wrist.bucketWrist(); + arm.toBucketPosition(); + wrist.toBucketPosition(); caseState = 6; } break; 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 new file mode 100644 index 0000000..cde8fe3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java @@ -0,0 +1,165 @@ +package org.firstinspires.ftc.teamcode.runmodes; + +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.states.FieldStates; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem; +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; + +public class DevTeleopRunMode { + + /* + Subsystems + */ + private MotorsSubsystem motors; + public ClawSubsystem claw; + public ArmSubsystem arm; + public WristSubsystem wrist; + public LiftSubsystem lift; + + /* + Controllers + */ + public Gamepad GP1; + public Gamepad GP2; + public Gamepad currentGP1; + public Gamepad previousGP1; + public Gamepad currentGP2; + public Gamepad previousGP2; + public FieldStates fieldStates; + public DevTeleopRunMode(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { + this.motors = new MotorsSubsystem(hardwareMap, telemetry); + 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.currentGP1 = new Gamepad(); + this.currentGP2 = new Gamepad(); + this.previousGP1 = new Gamepad(); + this.previousGP2 = new Gamepad(); + this.fieldStates = new FieldStates(); + } + + public void init() { + this.motors.init(); + this.claw.init(); + this.arm.init(); + this.wrist.init(); + this.lift.init(); + } + + public void update() { + this.previousGP1.copy(currentGP1); + this.currentGP1.copy(this.GP1); + this.previousGP2.copy(currentGP2); + this.currentGP2.copy(this.GP2); + this.theDrop(); + this.theLift(); + this.thePickup(); + this.theTravel(); + this.theLowBucketScore(); + this.theHighBucketScore(); + } + + /* + Controller: 1 + Button: A + 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) { + Actions.runBlocking(new SequentialAction( + this.arm.toFloorPosition, + this.wrist.toFloorPosition + )); + } + } + } + + /* + Controller: 1 + Button: Right Bumper + Action: On button press, open and closes claw + */ + public void thePickup() { + if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) { + this.claw.switchState(); + } + } + + /* + 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) { + Actions.runBlocking(new SequentialAction( + this.arm.toParkPosition, + this.wrist.toBucketPosition + )); + } + } + + /* + Controller: 2 + Button: Y + Action: On button press, lift to low bucket height, + arm to bucket position, wrist to bucket position + */ + public void theLowBucketScore() { + if (this.currentGP2.y && !this.previousGP2.y) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + lift.toLowBucket, + arm.toBucketPosition, + wrist.toBucketPosition + )); + } + } + + /* + Controller: 2 + Button: A + Action: On button press, lift to low bucket height, + arm to bucket position, wrist to bucket position + */ + public void theHighBucketScore() { + if (this.currentGP2.a && !this.previousGP2.a) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + lift.toHighBucket, + arm.toBucketPosition, + wrist.toBucketPosition + )); + } + } + + /* + Controller: 2 + Button: Direction Pad DOWN + Action: On directional press, lift to floor height, + arm to bucket position, wrist to floor position + */ + public void theTravel(){ + if (this.currentGP2.dpad_down && !this.previousGP2.dpad_down){ + fieldStates.setFieldLocation(FieldStates.FieldLocation.FIELD); + Actions.runBlocking(new SequentialAction( + lift.toFloor, + arm.toBucketPosition, + wrist.toFloorPosition + )); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java index 40195e4..eae8c58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java @@ -36,7 +36,7 @@ public class Teleop { public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) { - claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); + claw = new ClawSubsystem(hardwareMap, telemetry); initMotors(hardwareMap); this.follower = follower; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java new file mode 100644 index 0000000..2a2594f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.states; + +public class FieldStates { + + public enum FieldLocation { + BUCKET, SUBMARINE, FIELD + } + + private FieldLocation fieldLocation; + + public FieldLocation getFieldLocation() { + return fieldLocation; + } + + public void setFieldLocation(FieldLocation fieldLocation) { + this.fieldLocation = fieldLocation; + } +} 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 4e828e9..00dd17e 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 @@ -1,68 +1,66 @@ package org.firstinspires.ftc.teamcode.subsystem; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.ServoImplEx; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.util.action.Actions; import org.firstinspires.ftc.teamcode.util.action.RunAction; public class ArmSubsystem { public enum ArmState { - PARK, ENGAGE, BUCKET + PARK, FLOOR, BUCKET } - public ServoImplEx arm; - public ArmState state; - public RunAction engageArm, parkArm, bucketArm; + private ServoImplEx arm; + private ArmState state; + public RunAction toFloorPosition, toParkPosition, toBucketPosition; + private Telemetry telemetry; - public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) { - arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); - arm.resetDeviceConfigurationForOpMode(); - - this.state = armState; - - parkArm = new RunAction(this::parkArm); - engageArm = new RunAction(this::engageArm); - bucketArm = new RunAction(this::bucketArm); + public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); + this.toParkPosition = new RunAction(this::toParkPosition); + this.toFloorPosition = new RunAction(this::toFloorPosition); + this.toBucketPosition = new RunAction(this::toBucketPosition); + this.telemetry = telemetry; } public void setState(ArmState armState) { - if (armState == ArmState.ENGAGE) { - arm.setPosition(armEngage); - this.state = ArmState.ENGAGE; + if (armState == ArmState.FLOOR) { + this.arm.setPosition(armFloor); } else if (armState == ArmState.PARK) { - arm.setPosition(armPark); - this.state = ArmState.PARK; + this.arm.setPosition(armPark); } else if (armState == ArmState.BUCKET) { - arm.setPosition(armBucket); - this.state = ArmState.BUCKET; + this.arm.setPosition(armBucket); } + this.state = armState; + this.telemetry.addData("Arm State", this.getState()); } - public void engageArm() { - setState(ArmState.ENGAGE); + public void toFloorPosition() { + this.setState(ArmState.FLOOR); } - public void parkArm() { - setState(ArmState.PARK); + public void toParkPosition() { + this.setState(ArmState.PARK); } - public void bucketArm() { - setState(ArmState.BUCKET); + public void toBucketPosition() { + this.setState(ArmState.BUCKET); } public void switchState() { - if (state == ArmState.ENGAGE) { - setState(ArmState.PARK); - } else if (state == ArmState.PARK) { - setState(ArmState.BUCKET); - } else if (state == ArmState.BUCKET) { - setState(ArmState.ENGAGE); + if (this.state == ArmState.FLOOR) { + this.setState(ArmState.PARK); + } else if (this.state == ArmState.PARK) { + this.setState(ArmState.BUCKET); + } else if (this.state == ArmState.BUCKET) { + this.setState(ArmState.FLOOR); } } @@ -71,11 +69,12 @@ public class ArmSubsystem { } public void init() { - Actions.runBlocking(parkArm); + this.arm.resetDeviceConfigurationForOpMode(); + Actions.runBlocking(this.toParkPosition); } public void start() { - Actions.runBlocking(parkArm); + Actions.runBlocking(this.toParkPosition); } public double getPosition() { 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 b508ee4..2e66491 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 @@ -1,12 +1,14 @@ package org.firstinspires.ftc.teamcode.subsystem; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen; + import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.util.action.Actions; import org.firstinspires.ftc.teamcode.util.action.RunAction; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose; public class ClawSubsystem { @@ -17,23 +19,23 @@ public class ClawSubsystem { private Servo claw; private ClawState state; public RunAction openClaw, closeClaw; + public Telemetry telemetry; - public ClawSubsystem(HardwareMap hardwareMap, ClawState clawState) { - claw = hardwareMap.get(Servo.class, "claw-servo"); - this.state = clawState; - - openClaw = new RunAction(this::openClaw); - closeClaw = new RunAction(this::closeClaw); + public ClawSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.claw = hardwareMap.get(Servo.class, "claw-servo"); + this.openClaw = new RunAction(this::openClaw); + this.closeClaw = new RunAction(this::closeClaw); + this.telemetry = telemetry; } public void setState(ClawState clawState) { if (clawState == ClawState.CLOSED) { claw.setPosition(clawClose); - this.state = ClawState.CLOSED; } else if (clawState == ClawState.OPEN) { claw.setPosition(clawOpen); - this.state = ClawState.OPEN; } + this.state = clawState; + this.telemetry.addData("Claw State", this.getState()); } public ClawState getState() { 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 ed54f00..5cbe29d 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 @@ -10,29 +10,35 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.util.action.Actions; import org.firstinspires.ftc.teamcode.util.action.RunAction; public class LiftSubsystem { public DcMotor lift; - public RunAction toFloor, toLowBucket, toHighBucket; + public RunAction toFloor, toLowBucket, toHighBucket, toFloat; public enum LiftState { FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT } private LiftState liftState; + private Telemetry telemetry; - public LiftSubsystem(HardwareMap hardwareMap) { + public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { lift = hardwareMap.get(DcMotor.class, "lift-motor"); toFloor = new RunAction(this::toFloor); toLowBucket = new RunAction(this::toLowBucket); toHighBucket = new RunAction(this::toHighBucket); + toFloat = new RunAction(this::toFloat); + this.telemetry = telemetry; } 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() { @@ -77,12 +83,14 @@ public class LiftSubsystem { this.liftState = liftState; } + private LiftState getState() { return this.liftState; } + public int getPosition() { return lift.getCurrentPosition(); } public void start() { - this.toFloor(); + Actions.runBlocking(toFloor); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java index e217312..fbeb73c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java @@ -26,7 +26,7 @@ public class MotorsSubsystem { public DcMotor backRightMotor; public enum TravelState { - PARKED, BUCKET, SUBMARINE + STOPPED, MOVING } public TravelState travelState; @@ -60,6 +60,8 @@ public class MotorsSubsystem { backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + this.setState(TravelState.STOPPED); } public void setFrontLeftMotorPower(double power) { @@ -115,8 +117,8 @@ public class MotorsSubsystem { // Show the elapsed game time and wheel power. this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); - this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); - this.telemetry.update(); + this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + this.telemetry.addData("Current State", this.getState()); } public void setState(TravelState travelState) { @@ -127,4 +129,12 @@ public class MotorsSubsystem { return this.travelState; } + public void setPower(DcMotor motor, double power) { + motor.setPower(power); + if (power < 0.05) { + this.setState(TravelState.MOVING); + } else { + this.setState(TravelState.STOPPED); + } + } } 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 1e895dc..a5637db 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 @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.ServoImplEx; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.util.action.Actions; import org.firstinspires.ftc.teamcode.util.action.RunAction; @@ -17,33 +18,31 @@ public class WristSubsystem { public ServoImplEx wrist; public WristState state; - public RunAction floorWrist, bucketWrist; + public RunAction toFloorPosition, toBucketPosition; + public Telemetry telemetry; - public WristSubsystem(HardwareMap hardwareMap, WristState wristState) { - wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo"); - wrist.resetDeviceConfigurationForOpMode(); - - this.state = wristState; - - bucketWrist = new RunAction(this::bucketWrist); - floorWrist = new RunAction(this::floorWrist); + public WristSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo"); + this.telemetry = telemetry; + toBucketPosition = new RunAction(this::toBucketPosition); + toFloorPosition = new RunAction(this::toFloorPosition); } public void setState(WristState wristState) { if (wristState == WristState.FLOOR) { wrist.setPosition(wristFloor); - this.state = WristState.FLOOR; } else if (wristState == WristState.BUCKET) { wrist.setPosition(wristBucket); - this.state = WristState.BUCKET; } + this.state = wristState; + this.telemetry.addData("Wrist State", this.getState()); } - public void floorWrist() { + public void toFloorPosition() { setState(WristState.FLOOR); } - public void bucketWrist() { + public void toBucketPosition() { setState(WristState.BUCKET); } @@ -60,11 +59,12 @@ public class WristSubsystem { } public void init() { - Actions.runBlocking(floorWrist); + wrist.resetDeviceConfigurationForOpMode(); + Actions.runBlocking(toFloorPosition); } public void start() { - Actions.runBlocking(floorWrist); + Actions.runBlocking(toFloorPosition); } public double getPosition() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java index 5e40e32..d6203de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java @@ -15,9 +15,7 @@ public class Actions { while (b && !Thread.currentThread().isInterrupted()) { TelemetryPacket p = new TelemetryPacket(); p.fieldOverlay().getOperations().addAll(c.getOperations()); - b = a.run(p); - dash.sendTelemetryPacket(p); } }