We scored one point!

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

@@ -0,0 +1,150 @@
+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 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;
+
+@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 DcMotor frontLeftMotor;
+    public DcMotor backLeftMotor;
+    public DcMotor frontRightMotor;
+    public DcMotor backRightMotor;
+    @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();
+    }
+
+    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);
+
+        theDrop(arm, wrist);
+        thePickup(claw);
+        theLift(arm, wrist);
+        theLowBucketScore(lift, wrist, arm);
+
+        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);
+        frontRightMotor.setPower(rightFrontPower);
+        backLeftMotor.setPower(leftBackPower);
+        backRightMotor.setPower(rightBackPower);
+
+        // 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.update();
+
+    }
+}

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

@@ -64,6 +64,7 @@ public class LiftTest extends LinearOpMode {
         Gamepad currentGamepad1 = new Gamepad();
         Gamepad previousGamepad1 = new Gamepad();
 
+        lift.init();
         waitForStart();
         runtime.reset();
 

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

@@ -4,15 +4,15 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public static double clawClose = 0.95;
+    public static double clawClose = 1.00;
-    public static double clawOpen = 0.5;
+    public static double clawOpen = 0.25;
 
     public static double armEngage = 0.5;
     public static double armPark = 0.125;
     public static double armBucket = 0.175;
 
     public static double wristFloor = 0.625;
-    public static double wristBucket = 0.25;
+    public static double wristBucket = 0.215;
     public static int liftZeroPos = 0;
     public static int liftToLowBucketPos = 2250;
     public static int liftToHighBucketPos = 3700;

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

@@ -34,8 +34,8 @@ public class LiftSubsystem {
     }
 
     public void toZero() {
-        setTarget(liftZeroPos);
+        lift.setTargetPosition(liftZeroPos);
-        setState(LiftState.FLOOR);
+        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
     }
 
     public void switchState() {
@@ -49,13 +49,13 @@ public class LiftSubsystem {
     }
 
     public void toLowBucket() {
-        setTarget(liftToLowBucketPos);
+        lift.setTargetPosition(liftToLowBucketPos);
-        setState(LiftState.LOW_BUCKET);
+        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
     }
 
     public void toHighBucket() {
-        setTarget(liftToHighBucketPos);
+        lift.setTargetPosition(liftToHighBucketPos);
-        setState(LiftState.HIGH_BUCKET);
+        lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
     }
 
     public void init() {