Merge remote-tracking branch 'origin/branch-rc-chassis-14493-subsystem' into branch-rc-chassis-14493-subsystem

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java

@@ -0,0 +1,97 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+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;
+
+@TeleOp(name = "Dev Teleop Remix", group = "Debug")
+@Disabled
+public class DevTeleOpRemix extends OpMode {
+
+    public ClawSubsystem claw;
+    public ArmSubsystem arm;
+    public WristSubsystem wrist;
+    public LiftSubsystem lift;
+    public MotorsSubsystem motors;
+
+    public Gamepad currentGamepad1;
+    public Gamepad previousGamepad1;
+    public Gamepad currentGamepad2;
+    public Gamepad previousGamepad2;
+
+    public double 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);
+        motors = new MotorsSubsystem(hardwareMap, telemetry, power);
+
+        claw.init();
+        arm.init();
+        wrist.init();
+        lift.init();
+        motors.init();
+
+        currentGamepad1 = new Gamepad();
+        previousGamepad1 = new Gamepad();
+        currentGamepad2 = new Gamepad();
+        previousGamepad2 = new Gamepad();
+    }
+
+    public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
+        if (currentGamepad1.a && !previousGamepad1.a) {
+            wrist.floorWrist();
+            arm.engageArm();
+        }
+    }
+
+    public void thePickup(ClawSubsystem claw) {
+        if (currentGamepad1.x && !previousGamepad1.x) {
+            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 (currentGamepad1.y && !previousGamepad1.y) {
+            lift.toLowBucket();
+            wrist.bucketWrist();
+        }
+    }
+
+    @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);
+
+        motors.calculateTrajectory(gamepad1);
+
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java

@@ -0,0 +1,107 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+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;
+
+@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug")
+@Disabled
+public class DevTeleOpRemixDeux extends OpMode {
+
+    private Follower follower;
+
+    public ClawSubsystem claw;
+    public ArmSubsystem arm;
+    public WristSubsystem wrist;
+    public LiftSubsystem lift;
+    public MotorsSubsystem motors;
+
+    public Gamepad currentGamepad1;
+    public Gamepad previousGamepad1;
+    public Gamepad currentGamepad2;
+    public Gamepad previousGamepad2;
+
+    public double power = .6;
+
+    @Override
+    public void init() {
+
+        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);
+        motors = new MotorsSubsystem(hardwareMap, telemetry);
+        lift = new LiftSubsystem(hardwareMap);
+
+        claw.init();
+        arm.init();
+        wrist.init();
+        lift.init();
+        motors.init();
+
+        currentGamepad1 = new Gamepad();
+        previousGamepad1 = new Gamepad();
+        currentGamepad2 = new Gamepad();
+        previousGamepad2 = new Gamepad();
+
+        follower.setMaxPower(this.power);
+        follower.startTeleopDrive();
+
+    }
+
+    public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
+        if (currentGamepad1.a && !previousGamepad1.a) {
+            wrist.floorWrist();
+            arm.engageArm();
+        }
+    }
+
+    public void thePickup(ClawSubsystem claw) {
+        if (currentGamepad1.x && !previousGamepad1.x) {
+            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 (currentGamepad1.y && !previousGamepad1.y) {
+            lift.toLowBucket();
+            wrist.bucketWrist();
+        }
+    }
+
+    @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);
+
+        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
+        follower.update();
+
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java

@@ -27,7 +27,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.cometbots.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java

@@ -27,7 +27,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.cometbots.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java

@@ -27,7 +27,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.cometbots.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java

@@ -27,12 +27,10 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.cometbots.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
@@ -76,7 +74,7 @@ public class LiftTest extends LinearOpMode {
             currentGamepad1.copy(gamepad1);
 
             if (currentGamepad1.square && !previousGamepad1.square) {
-                lift.toZero();
+                lift.toFloor();
             }
 
             if (currentGamepad1.triangle && !previousGamepad1.triangle) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java

@@ -27,7 +27,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.cometbots.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -13,7 +13,8 @@ public class RobotConstants {
 
     public static double wristFloor = 0.625;
     public static double wristBucket = 0.215;
-    public static int liftZeroPos = 0;
+    public static int liftToFloorPos = 0;
+    public static int liftToPoolPos = 500;
     public static int liftToLowBucketPos = 2250;
     public static int liftToHighBucketPos = 3700;
     public static double liftPower = .45;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java

@@ -33,6 +33,8 @@ public class Auto {
     public Follower follower;
     public Telemetry telemetry;
 
+    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);
@@ -61,23 +63,52 @@ public class Auto {
     }
 
     public void update() {
-        //follower.update();
+
-        if (clawTimer.getElapsedTimeSeconds() > 2) {
+        this.telemetry.addData("Current State", caseState);
+        this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
+        this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
+        this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
+        this.telemetry.update();
+
+        switch(caseState) {
+            case 1:
                 claw.openClaw();
-        }
+                caseState = 2;
-        if (armTimer.getElapsedTimeSeconds() > 4) {
+                break;
+            case 2:
+                if (clawTimer.getElapsedTimeSeconds() > 2) {
                     arm.engageArm();
+                    caseState = 3;
                 }
+                break;
+            case 3:
+                if (armTimer.getElapsedTimeSeconds() > 4) {
+                    wrist.floorWrist();
+                    caseState = 4;
+                }
+                break;
+            case 4:
                 if (clawTimer.getElapsedTimeSeconds() > 6) {
                     claw.closeClaw();
+                    caseState = 5;
                 }
+                break;
+            case 5:
                 if (armTimer.getElapsedTimeSeconds() > 8) {
                     arm.bucketArm();
                     wrist.bucketWrist();
+                    caseState = 6;
                 }
+                break;
+            case 6:
                 if (clawTimer.getElapsedTimeSeconds() > 10) {
                     claw.openClaw();
+                    caseState = 7;
+                }
+                break;
+            case 7:
+                this.init();
+                break;
         }
     }
-
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
-import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftZeroPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -14,16 +14,17 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction;
 public class LiftSubsystem {
 
     public DcMotor lift;
-    public RunAction toZero, toLowBucket, toHighBucket;
+    public RunAction toFloor, toLowBucket, toHighBucket;
+
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL
     }
+
     private LiftState liftState;
 
     public LiftSubsystem(HardwareMap hardwareMap) {
         lift = hardwareMap.get(DcMotor.class, "lift-motor");
-
+        toFloor = new RunAction(this::toFloor);
-        toZero = new RunAction(this::toZero);
         toLowBucket = new RunAction(this::toLowBucket);
         toHighBucket = new RunAction(this::toHighBucket);
     }
@@ -33,29 +34,36 @@ public class LiftSubsystem {
         lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
     }
 
-    public void toZero() {
-        lift.setTargetPosition(liftZeroPos);
-        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-    }
-
     public void switchState() {
-        if(this.liftState == LiftState.FLOOR) {
+        if (this.liftState == LiftState.FLOOR) {
+            this.toPool();
+        } else if (this.liftState == LiftState.POOL) {
             this.toLowBucket();
         } else if (this.liftState == LiftState.LOW_BUCKET) {
             this.toHighBucket();
         } else if (this.liftState == LiftState.HIGH_BUCKET) {
-            this.toZero();
+            this.toFloor();
         }
     }
 
+    public void toFloor() {
+        this.setTarget(liftToPoolPos);
+        this.setState(LiftState.FLOOR);
+    }
+
+    public void toPool() {
+        this.setTarget(liftToPoolPos);
+        this.setState(LiftState.POOL);
+    }
+
     public void toLowBucket() {
-        lift.setTargetPosition(liftToLowBucketPos);
+        this.setTarget(liftToLowBucketPos);
-        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        this.setState(LiftState.LOW_BUCKET);
     }
 
     public void toHighBucket() {
-        lift.setTargetPosition(liftToHighBucketPos);
+        this.setTarget(liftToHighBucketPos);
-        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        this.setState(LiftState.HIGH_BUCKET);
     }
 
     public void init() {
@@ -73,7 +81,7 @@ public class LiftSubsystem {
     }
 
     public void start() {
-        setTarget(10);
+        this.toFloor();
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java

@@ -0,0 +1,117 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+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 com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class MotorsSubsystem {
+
+    public HardwareMap hardwareMap;
+
+    public Telemetry telemetry;
+
+    public DcMotor frontLeftMotor;
+    public DcMotor backLeftMotor;
+    public DcMotor frontRightMotor;
+    public DcMotor backRightMotor;
+
+    public double power;
+
+    public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
+        this.hardwareMap = hardwareMap;
+        this.telemetry = telemetry;
+        this.power = 1.0;
+    }
+
+    public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
+        this.hardwareMap = hardwareMap;
+        this.telemetry = telemetry;
+        this.power = power;
+    }
+
+    public void 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);
+
+        frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void setFrontLeftMotorPower(double power) {
+        frontLeftMotor.setPower(power);
+    }
+
+    public void setBackLeftMotorPower(double power) {
+        backLeftMotor.setPower(power);
+    }
+
+    public void setFrontRightMotorPower(double power) {
+        frontRightMotor.setPower(power);
+    }
+
+    public void setBackRightMotorPower(double power) {
+        backRightMotor.setPower(power);
+    }
+
+    public void calculateTrajectory(Gamepad gamepad1) {
+        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 leftBackPower = axial - lateral + yaw;
+
+        double rightFrontPower = 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
+        this.setFrontLeftMotorPower(leftFrontPower * this.power);
+        this.setFrontRightMotorPower(rightFrontPower * this.power);
+        this.setBackLeftMotorPower(leftBackPower * this.power);
+        this.setBackRightMotorPower(rightBackPower * this.power);
+
+        // 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();
+    }
+
+}