Working code for Meet 1

TeamCode/src/main/java/opmodes/MainTeleOp.java

@@ -35,13 +35,8 @@ public class MainTeleOp extends OpMode {
     @Override
     public void loop() {
         // Drive
-        double x = -gamepad1.left_stick_x;
+        boolean slowmode = gamepad1.right_bumper;
-        double y = -gamepad1.left_stick_y;
+        this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
-        double z = -gamepad1.right_stick_x;
-        this.robot.getDrive().setInput(0, y, z);
-
-        this.telemetry.addLine(this.robot.getDrive().getTelemetry());
-        this.telemetry.update();
 
         // Button Mappings
         boolean openClaw = gamepad2.b; // B
@@ -133,11 +128,12 @@ public class MainTeleOp extends OpMode {
             this.robot.getLift().stopReset();
         }
 
-        this.robot.update();
 
         this.previousSlideUp = slideUp;
         this.previousScrewArmToggle = screwArmToggle;
         this.previousRobotLiftReset = robotLiftReset;
         this.previousRobotLiftExtend = robotLiftExtend;
+
+        this.robot.update();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_LEFT_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_KP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MIN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_RIGHT_NAME;
@@ -12,12 +13,12 @@ import com.arcrobotics.ftclib.controller.PController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
 public class Claw {
     private final Servo claw;
     private final Servo armLeft;
     private final Servo armRight;
+    PController clawController = new PController(CLAW_KP);
+    private double clawControllerTarget;
 
     public Claw(HardwareMap hardwareMap) {
         this.claw = hardwareMap.get(Servo.class, CLAW_NAME);
@@ -29,11 +30,11 @@ public class Claw {
     }
 
     public void open() {
-        this.claw.setPosition(CLAW_MAX);
+        this.clawControllerTarget = CLAW_MAX;
     }
 
     public void close() {
-        this.claw.setPosition(CLAW_MIN);
+        this.clawControllerTarget = CLAW_MIN;
     }
 
     public void setArmPosition(double target) {
@@ -41,4 +42,16 @@ public class Claw {
         this.armLeft.setPosition(target);
         this.armRight.setPosition(target);
     }
+
+    public void update() {
+        this.clawController.setSetPoint(this.clawControllerTarget);
+        this.clawController.setTolerance(0.001);
+        this.clawController.setP(CLAW_KP);
+
+        double output = 0;
+        if (!this.clawController.atSetPoint()) {
+            output = Math.max(-1 * CLAW_MAX, Math.min(CLAW_MAX, this.clawController.calculate(claw.getPosition())));
+            this.claw.setPosition(this.claw.getPosition() + output);
+        }
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -9,11 +10,8 @@ import lombok.Getter;
 
 public class Robot {
 
-//    @Getter
-//    private MecanumDrive drive;
-
     @Getter
-    private Drive drive;
+    private MecanumDrive drive;
 
     @Getter
     private Gantry gantry;
@@ -32,8 +30,7 @@ public class Robot {
     }
 
     private void init(HardwareMap hardwareMap) {
-//        this.drive = new MecanumDrive(hardwareMap);
+        this.drive = new MecanumDrive(hardwareMap);
-        this.drive = new Drive(hardwareMap);
         this.gantry = new Gantry(hardwareMap, telemetry);
         this.claw = new Claw(hardwareMap);
         this.lift = new RobotLift(hardwareMap, telemetry);
@@ -43,5 +40,11 @@ public class Robot {
         this.gantry.update();
         this.lift.update();
         this.telemetry.update();
+        this.drive.update();
+        this.claw.update();
+
+//        Pose2d pose = this.drive.getLocalizer().getPoseEstimate();
+//        this.telemetry.addData("x", pose.getX());
+//        this.telemetry.addData("y", pose.getY());
     }
 }

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

@@ -19,34 +19,43 @@ public class RobotConstants {
     public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation";
     public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
     public static final String WEBCAM_NAME = "webcam";
+    public static final String PARALLEL_DRIVE_ODOMETRY = BACK_RIGHT_NAME;
+    public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_LEFT_NAME;
+
+    // Drive
+    public static double SLOW_MODE_SPEED_PCT = 0.25;
+    public static double SLOW_MODE_TURN_PCT = 0.25;
+    public static double SPEED = 1f;
+    public static double TURN = 1f;
 
     // Slide
     public static int SLIDE_UP = 100;
 
     // Arm
-    public static double PICKUP_ARM_MIN = 0.17;
+    public static double PICKUP_ARM_MIN = 0.185;
     public static double PICKUP_ARM_MAX = 0.76;
-    public static double CLAW_MIN = 0.9;
+    public static double CLAW_MIN = 0.92;
     public static double CLAW_MAX = 0.6;
-    public static double CLAW_ARM_DELTA = 0.008;
+    public static double CLAW_ARM_DELTA = 0.03;
 
     // Gantry
-    public static double GANTRY_ARM_MIN = 0.43;
+    public static double GANTRY_ARM_MIN = 0.435;
-    public static double GANTRY_ARM_MAX = 0.95;
+    public static double GANTRY_ARM_MAX = 0.92;
     public static int GANTRY_LIFT_DELTA = 50;
     public static double GANTRY_X_DELTA = 0.01;
     public static double GANTRY_CENTER = 0.5;
-    public static double GANTRY_ARM_KP = 0.03;
+    public static double GANTRY_ARM_KP = 0.06;
     public static double GANTRY_ARM_KI = 0f;
     public static double GANTRY_ARM_KD = 0f;
-    public static double GANTRY_ARM_DELTA_MAX = 0.0025;
+    public static double GANTRY_ARM_DELTA_MAX = 0.013;
 
     // Robot Lift
     public static double LIFT_ROTATION_UP = 0.4;
     public static double LIFT_ROTATION_DOWN = 0;
     public static int LIFT_EXTEND_MAX = 13100;
-    public static double LIFT_RETRACT_PCT = 0.1;
+    public static double LIFT_RETRACT_PCT = 0.3;
-    public static double LIFT_ARM_KP = 0.038;
+    public static double LIFT_ARM_KP = 0.1;
     public static double LIFT_POWER = 1f;
 
+    public static double CLAW_KP = 0.3;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java

@@ -1,5 +1,13 @@
 package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
 
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_LEFT_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_RIGHT_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_LEFT_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_RIGHT_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLOW_MODE_SPEED_PCT;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLOW_MODE_TURN_PCT;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SPEED;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.TURN;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ACCEL;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_VEL;
@@ -29,6 +37,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@@ -40,15 +49,16 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil;
+
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
+
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
 
-
 /*
  * Simple mecanum drive hardware implementation for REV hardware.
  */
@@ -99,10 +109,10 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
                 DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
         imu.initialize(parameters);
 
-        leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+        leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_NAME);
-        leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
+        leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_NAME);
-        rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
+        rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_NAME);
-        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+        rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_NAME);
 
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 
@@ -124,11 +134,16 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
 
         // TODO: reverse any motors using DcMotor.setDirection()
 
+        this.leftFront.setDirection(DcMotor.Direction.REVERSE);
+        this.rightFront.setDirection(DcMotor.Direction.FORWARD);
+        this.leftRear.setDirection(DcMotor.Direction.REVERSE);
+        this.rightRear.setDirection(DcMotor.Direction.FORWARD);
+
         List<Integer> lastTrackingEncPositions = new ArrayList<>();
         List<Integer> lastTrackingEncVels = new ArrayList<>();
 
         // TODO: if desired, use setLocalizer() to change the localization method
-        // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels));
+        setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
 
         trajectorySequenceRunner = new TrajectorySequenceRunner(
                 follower, HEADING_PID, batteryVoltageSensor,
@@ -309,9 +324,15 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
         return new ProfileAccelerationConstraint(maxAccel);
     }
 
-    public void setInput(double x, double y, double z) {
+    public void setInput(Gamepad gamepad1, Gamepad gamepad2, boolean slowmode) {
-        Pose2d vel = new Pose2d(x, y, z);
+        double speedScale = slowmode ? SLOW_MODE_SPEED_PCT : SPEED;
-        setDrivePower(vel);
+        double turnScale = slowmode ? SLOW_MODE_TURN_PCT : TURN;
-        update();
+
+        this.setWeightedDrivePower(
+                new Pose2d(
+                        gamepad1.left_stick_y * -1 * speedScale,
+                        gamepad1.left_stick_x * -1 * speedScale,
+                        -gamepad1.right_stick_x * turnScale
+                ));
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java

@@ -0,0 +1,110 @@
+package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
+
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PARALLEL_DRIVE_ODOMETRY;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PERPENDICULAR_DRIVE_ODOMETRY;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ *    ^
+ *    |
+ *    | ( x direction)
+ *    |
+ *    v
+ *    <----( y direction )---->
+
+ *        (forward)
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |    <- Perpendicular Wheel
+ *    |           || |
+ *    |           || |    <- Parallel Wheel
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *
+ */
+public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
+    public static double TICKS_PER_REV = 2000;
+    public static double WHEEL_RADIUS = 0.944882; // in
+    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+    public static double PARALLEL_X = 0.269685; // X is the up and down direction
+    public static double PARALLEL_Y = 4.409449; // Y is the strafe direction
+
+    public static double PERPENDICULAR_X = -0.6299213;
+    public static double PERPENDICULAR_Y = 0.1122047;
+
+    public static double X_MULTIPLIER = 1; // Multiplier in the X direction
+    public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction
+    // Parallel/Perpendicular to the forward axis
+    // Parallel wheel is parallel to the forward axis
+    // Perpendicular is perpendicular to the forward axis
+    private final Encoder parallelEncoder;
+    private final Encoder perpendicularEncoder;
+
+    private final MecanumDrive drive;
+
+    public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, MecanumDrive drive) {
+        super(Arrays.asList(
+                new Pose2d(PARALLEL_X, PARALLEL_Y, 0),
+                new Pose2d(PERPENDICULAR_X, PERPENDICULAR_Y, Math.toRadians(90))
+        ));
+
+        this.drive = drive;
+
+        parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, PARALLEL_DRIVE_ODOMETRY));
+        perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, PERPENDICULAR_DRIVE_ODOMETRY));
+
+        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+        parallelEncoder.setDirection(Encoder.Direction.REVERSE);
+    }
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    @Override
+    public double getHeading() {
+        return drive.getRawExternalHeading();
+    }
+
+    @Override
+    public Double getHeadingVelocity() {
+        return drive.getExternalHeadingVelocity();
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        return Arrays.asList(
+                encoderTicksToInches(parallelEncoder.getCurrentPosition()) * X_MULTIPLIER,
+                encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) * Y_MULTIPLIER
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+        //  compensation method
+
+        return Arrays.asList(
+                encoderTicksToInches(parallelEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
+                encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
+        );
+    }
+}