Festive Potatoes and I got the Octoquad working (#72)

* OctoQuad based odo. Wholly untested. * Added the ability to flip between OctoQuad and Encoder ports * Refactored into an IEncoder interface. Nothing is tested on hardware. * Cleaned up the 20403 drivebase a little * Adjusted the 4 test paths to see if the odo & drivebase are working * Final minor tidying * Tested on the Bot
technototes · Nov 16, 2024 · 2fe8a98 · 2fe8a98
1 parent 9bbdf94
commit 2fe8a98
Show file tree

Hide file tree

Showing 11 changed files with 134 additions and 100 deletions.
diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java
@@ -13,11 +13,11 @@
 public class AutoConstants {
 
     // Stuff for testing:
-    public static ConfigurablePoseD TEST_START = new ConfigurablePoseD(0, 0, 0);
-    public static ConfigurablePoseD TEST_LEFT = new ConfigurablePoseD(48, 0, 0);
-    public static ConfigurablePoseD TEST_FORWARD = new ConfigurablePoseD(0, 48, 0);
-    public static ConfigurablePoseD TEST_SPLINE_1 = new ConfigurablePoseD(24, 48, 0);
-    public static ConfigurablePoseD TEST_SPLINE_2 = new ConfigurablePoseD(24, 48, -90);
+    public static ConfigurablePoseD TEST_START = new ConfigurablePoseD(-50, -50, 0);
+    public static ConfigurablePoseD TEST_LEFT = new ConfigurablePoseD(-2, -50, 0);
+    public static ConfigurablePoseD TEST_FORWARD = new ConfigurablePoseD(-50, -2, 0);
+    public static ConfigurablePoseD TEST_SPLINE_1 = new ConfigurablePoseD(-38, -2, 0);
+    public static ConfigurablePoseD TEST_SPLINE_2 = new ConfigurablePoseD(-38, -2, -90);
     public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> TEST_RIGHT_TO_LEFT =
         func -> func.apply(TEST_START.toPose())
             .lineToLinearHeading(TEST_LEFT.toPose())
@@ -36,13 +36,13 @@ public class AutoConstants {
             .build();
     public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> TEST_SPLINE_PATH_1 =
         func -> func.apply(TEST_START.toPose())
-            .splineToLinearHeading(TEST_SPLINE_1.toPose(), TEST_FORWARD.getHeading())
-            .splineToLinearHeading(TEST_START.toPose(), TEST_FORWARD.getHeading())
+            .splineToLinearHeading(TEST_SPLINE_1.toPose(), TEST_START.getHeading())
+            .splineToLinearHeading(TEST_START.toPose(), TEST_START.getHeading())
             .build();
     public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> TEST_SPLINE_PATH_2 =
         func -> func.apply(TEST_START.toPose())
-            .splineToSplineHeading(TEST_SPLINE_2.toPose(), TEST_FORWARD.getHeading())
-            .splineToSplineHeading(TEST_START.toPose(), TEST_SPLINE_2.getHeading())
+            .splineToSplineHeading(TEST_SPLINE_2.toPose(), Math.toRadians(90))
+            .splineToSplineHeading(TEST_START.toPose(), Math.toRadians(-45))
             .build();
 
     public static ConfigurablePoseD OBSERVATION_START = new ConfigurablePoseD(0, 60, -90);

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java
@@ -21,14 +21,16 @@
 import com.technototes.vision.hardware.Webcam;
 import java.util.List;
 import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit;
+import org.firstinspires.ftc.twenty403.helpers.IEncoder;
+import org.firstinspires.ftc.twenty403.helpers.OctoquadEncoder;
 
 public class Hardware implements Loggable {
 
     public List<LynxModule> hubs;
 
     public IGyro imu;
     public EncodedMotor<DcMotorEx> fl, fr, rl, rr, armL, armR;
-    public MotorEncoder odoF, odoR;
+    public IEncoder odoF, odoR;
     public Servo retainer, jaw, wrist;
     public CRServo intake;
     public ColorSensor colorSensor;
@@ -56,10 +58,6 @@ public Hardware(HardwareMap hwmap) {
             rl = new EncodedMotor<>(Setup.HardwareNames.RLMOTOR);
             rr = new EncodedMotor<>(Setup.HardwareNames.RRMOTOR);
         }
-        if (Setup.Connected.ODOSUBSYSTEM) {
-            odoR = new MotorEncoder(Setup.HardwareNames.ODOR);
-            odoF = new MotorEncoder(Setup.HardwareNames.ODOF);
-        }
         if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM) {
             intake = new CRServo(Setup.HardwareNames.INTAKE);
             retainer = new Servo(Setup.HardwareNames.RETAINER);
@@ -75,7 +73,14 @@ public Hardware(HardwareMap hwmap) {
             rotate1 = new EncodedMotor<>(Setup.HardwareNames.ARML);
             rotate2 = new EncodedMotor<>(Setup.HardwareNames.ARMR);
             slides = new EncodedMotor<>(Setup.HardwareNames.SLIDEMOTOR);
+        }
+        if (Setup.Connected.OCTOQUAD) {
             octoquad = hwmap.get(OctoQuad.class, Setup.HardwareNames.OCTOQUAD);
+            octoquad.resetAllPositions();
+            if (Setup.Connected.ODOSUBSYSTEM) {
+                odoR = new OctoquadEncoder(octoquad, Setup.OctoQuadPorts.ODOR);
+                odoF = new OctoquadEncoder(octoquad, Setup.OctoQuadPorts.ODOF);
+            }
         }
     }
 

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Robot.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Robot.java
@@ -3,6 +3,7 @@
 import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
 import com.technototes.library.logger.Loggable;
 import com.technototes.library.util.Alliance;
+import org.firstinspires.ftc.twenty403.helpers.OctoquadEncoder;
 import org.firstinspires.ftc.twenty403.helpers.StartingPosition;
 import org.firstinspires.ftc.twenty403.subsystems.ArmSubsystem;
 import org.firstinspires.ftc.twenty403.subsystems.DrivebaseSubsystem;
@@ -28,32 +29,20 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) {
         this.position = pos;
         this.alliance = team;
         this.initialVoltage = hw.voltage();
-        if (Setup.Connected.ODOSUBSYSTEM) {
-            this.localizer = new TwoDeadWheelLocalizer(hw.odoR, hw.odoF);
+        if (Setup.Connected.ODOSUBSYSTEM && Setup.Connected.OCTOQUAD) {
+            this.localizer = new TwoDeadWheelLocalizer(hw.odoF, hw.odoR, hw.imu);
         } else {
             this.localizer = null;
         }
         if (Setup.Connected.DRIVEBASE) {
-            if (localizer == null) {
-                this.drivebaseSubsystem = new DrivebaseSubsystem(
-                    hw.fl,
-                    hw.fr,
-                    hw.rl,
-                    hw.rr,
-                    hw.imu
-                );
-            } else {
-                this.drivebaseSubsystem = new DrivebaseSubsystem(
-                    hw.fl,
-                    hw.fr,
-                    hw.rl,
-                    hw.rr,
-                    hw.imu,
-                    localizer
-                );
-                // YOU MUST CALL THIS IMMEDIATELY AFTER CREATING THE DRIVEBASE!
-                localizer.setDrivebase(this.drivebaseSubsystem);
-            }
+            this.drivebaseSubsystem = new DrivebaseSubsystem(
+                hw.fl,
+                hw.fr,
+                hw.rl,
+                hw.rr,
+                hw.imu,
+                localizer
+            );
         }
         if (Setup.Connected.SAFETYSUBSYSTEM) {
             this.safetySubsystem = new SafetySubsystem(hw);

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java
@@ -8,12 +8,13 @@ public class Setup {
     public static class Connected {
 
         public static boolean DRIVEBASE = true;
-        public static boolean ODOSUBSYSTEM = false;
+        public static boolean ODOSUBSYSTEM = true;
         public static boolean SAFETYSUBSYSTEM = false;
         public static boolean KIDSSHAMPOOSUBSYSTEM = false;
         public static boolean HANGSUBSYSTEM = false;
         public static boolean ARMSUBSYSTEM = true;
         public static boolean EXTERNALIMU = true;
+        public static boolean OCTOQUAD = true;
     }
 
     @Config
@@ -43,9 +44,9 @@ public static class HardwareNames {
     @Config
     public static class OctoQuadPorts {
 
-        public static int ARMENCODER = 5;
-        public static int ODOR = 6; //TODO: verify with robot, r & l may be swapped
-        public static int ODOL = 7;
+        public static int ARMENCODER = 2;
+        public static int ODOR = 1; //TODO: verify with robot, r & l may be swapped
+        public static int ODOF = 0;
     }
 
     @Config

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/helpers/IEncoder.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/helpers/IEncoder.java
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.twenty403.helpers;
+
+// A simple interface to wrap up an Encoder interface
+public interface IEncoder {
+    void setDirection(boolean reversed);
+    int getPosition();
+    double getVelocity();
+}
diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/helpers/MotorEncoderPort.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/helpers/MotorEncoderPort.java
@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.twenty403.helpers;
+
+import com.technototes.library.hardware.sensor.encoder.MotorEncoder;
+import com.technototes.library.hardware.sensor.encoder.MotorEncoder.Direction;
+
+public class MotorEncoderPort implements IEncoder {
+
+    protected MotorEncoder motorPort;
+
+    public MotorEncoderPort(MotorEncoder motor) {
+        motorPort = motor;
+    }
+
+    @Override
+    public void setDirection(boolean reversed) {
+        motorPort.setDirection(reversed ? Direction.REVERSE : Direction.FORWARD);
+    }
+
+    @Override
+    public int getPosition() {
+        return motorPort.getCurrentPosition();
+    }
+
+    @Override
+    public double getVelocity() {
+        return motorPort.getCorrectedVelocity();
+    }
+}
diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/helpers/OctoquadEncoder.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/helpers/OctoquadEncoder.java
@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.twenty403.helpers;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase.EncoderDirection;
+
+public class OctoquadEncoder implements IEncoder {
+
+    protected OctoQuad octoQuad;
+    protected int portNumber;
+
+    public OctoquadEncoder(OctoQuad o, int port) {
+        octoQuad = o;
+        portNumber = port;
+    }
+
+    @Override
+    public void setDirection(boolean reversed) {
+        octoQuad.setSingleEncoderDirection(
+            portNumber,
+            reversed ? EncoderDirection.REVERSE : EncoderDirection.FORWARD
+        );
+    }
+
+    @Override
+    public int getPosition() {
+        return octoQuad.readSinglePosition(portNumber);
+    }
+
+    @Override
+    public double getVelocity() {
+        return octoQuad.readSingleVelocity(portNumber);
+    }
+}
diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java
@@ -69,7 +69,6 @@ public ArmSubsystem(Hardware hw) {
         rotate2.coast();
         slides = hw.slides;
         octoquad = hw.octoquad;
-        octoquad.resetAllPositions();
 
         armPidController = new PIDFController(
             armPID,

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/DrivebaseSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/DrivebaseSubsystem.java
@@ -10,7 +10,6 @@
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.technototes.library.hardware.motor.EncodedMotor;
 import com.technototes.library.hardware.sensor.IGyro;
-import com.technototes.library.hardware.sensor.IMU;
 import com.technototes.library.logger.Log;
 import com.technototes.library.logger.LogConfig;
 import com.technototes.library.logger.Loggable;
@@ -183,28 +182,7 @@ public DrivebaseSubsystem(
         fl.setDirection(DcMotorSimple.Direction.FORWARD);
         rl.setDirection(DcMotorSimple.Direction.FORWARD);
         rr.setDirection(DcMotorSimple.Direction.REVERSE);
-    }
-
-    public DrivebaseSubsystem(
-        EncodedMotor<DcMotorEx> fl,
-        EncodedMotor<DcMotorEx> fr,
-        EncodedMotor<DcMotorEx> rl,
-        EncodedMotor<DcMotorEx> rr,
-        IGyro i
-    ) {
-        super(fl, fr, rl, rr, i, () -> DriveConstants.class);
-        fl2 = fl;
-        fr2 = fr;
-        rl2 = rl;
-        rr2 = rr;
-        speed = DriveConstants.SLOW_MOTOR_SPEED;
-        // This is already handled in the parent class constructor (super)
-        setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
-
-        fl.setDirection(DcMotorSimple.Direction.REVERSE);
         fr.setDirection(DcMotorSimple.Direction.REVERSE);
-        rl.setDirection(DcMotorSimple.Direction.REVERSE);
-        rr.setDirection(DcMotorSimple.Direction.REVERSE);
     }
 
     @Override
@@ -222,9 +200,10 @@ public void periodic() {
             updatePoseEstimate();
             Pose2d pose = getPoseEstimate();
             Pose2d poseVelocity = getPoseVelocity();
-            poseDisplay = pose.toString() +
-            " : " +
-            (poseVelocity != null ? poseVelocity.toString() : "<null>");
+            poseDisplay =
+                pose.toString() +
+                " : " +
+                (poseVelocity != null ? poseVelocity.toString() : "<null>");
         }
         heading = gyro.getHeading();
     }

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/SafetySubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/SafetySubsystem.java
@@ -79,15 +79,15 @@ private int getOdoFPosition() {
         if (failedPart == FailedPart.ODOF) {
             return 0;
         } else {
-            return myHw.odoF.getCurrentPosition();
+            return myHw.odoF.getPosition();
         }
     }
 
     private int getOdoRPosition() {
         if (failedPart == FailedPart.ODOR) {
             return 0;
         } else {
-            return myHw.odoR.getCurrentPosition();
+            return myHw.odoR.getPosition();
         }
     }