TechnoLib 2.0 updates

Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java

@@ -123,10 +123,10 @@ public PathingMecanumDrivebaseSubsystem(
             c.getDouble(WheelBase.class),
             c.getDouble(LateralMult.class)
         );
-        leftFront = fl.getDevice();
-        leftRear = rl.getDevice();
-        rightRear = rr.getDevice();
-        rightFront = fr.getDevice();
+        leftFront = fl.getRawMotor(DcMotorEx.class);
+        leftRear = rl.getRawMotor(DcMotorEx.class);
+        rightRear = rr.getRawMotor(DcMotorEx.class);
+        rightFront = fr.getRawMotor(DcMotorEx.class);
 
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 

Path/src/main/java/com/technototes/path/subsystem/TankDrivebaseSubsystem.java

@@ -21,9 +21,8 @@
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
-import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
 import com.technototes.library.hardware.HardwareDevice;
-import com.technototes.library.hardware.motor.EncodedMotorGroup;
+import com.technototes.library.hardware.motor.EncodedMotor;
 import com.technototes.library.hardware.sensor.IMU;
 import com.technototes.library.subsystem.Subsystem;
 import com.technototes.path.subsystem.TankConstants.*;
@@ -34,7 +33,6 @@
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
-import java.util.stream.Collectors;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 
 public class TankDrivebaseSubsystem extends TankDrive implements Subsystem {
@@ -60,14 +58,14 @@ public class TankDrivebaseSubsystem extends TankDrive implements Subsystem {
 
     private TrajectoryFollower follower;
 
-    private List<DcMotorEx> motors, leftMotors, rightMotors;
+    private List<EncodedMotor<DcMotorEx>> motors, leftMotors, rightMotors;
     private IMU imu;
 
     private VoltageSensor batteryVoltageSensor;
 
     public TankDrivebaseSubsystem(
-        EncodedMotorGroup<DcMotorEx> left,
-        EncodedMotorGroup<DcMotorEx> right,
+        List<EncodedMotor<DcMotorEx>> left,
+        List<EncodedMotor<DcMotorEx>> right,
         IMU i,
         TankConstants c,
         Localizer localizer
@@ -120,17 +118,19 @@ public TankDrivebaseSubsystem(
         // add/remove motors depending on your robot (e.g., 6WD)
 
         motors = new ArrayList<>();
-        leftMotors = left.getAllDeviceList().stream().map(HardwareDevice::getDevice).collect(Collectors.toList());
-        rightMotors = right.getAllDeviceList().stream().map(HardwareDevice::getDevice).collect(Collectors.toList());
+        leftMotors = left;
+        rightMotors = right;
         motors.addAll(leftMotors);
         motors.addAll(rightMotors);
 
-        for (DcMotorEx motor : motors) {
+        /*
+         * Turning this off for now, because we don't expose it in the EncodedMotor class (yet?)
+        for (EncodedMotor<DcMotorEx> motor : motors) {
             MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
             motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
             motor.setMotorType(motorConfigurationType);
         }
-
+        */
         if (c.getBoolean(TankConstants.UseDriveEncoder.class)) {
             setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         }
@@ -215,14 +215,18 @@ public boolean isBusy() {
     }
 
     public void setMode(DcMotor.RunMode runMode) {
-        for (DcMotorEx motor : motors) {
-            motor.setMode(runMode);
+        for (EncodedMotor<DcMotorEx> motor : motors) {
+            motor.setRunMode(runMode);
         }
     }
 
     public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
-        for (DcMotorEx motor : motors) {
-            motor.setZeroPowerBehavior(zeroPowerBehavior);
+        for (EncodedMotor<DcMotorEx> motor : motors) {
+            if (zeroPowerBehavior == DcMotor.ZeroPowerBehavior.BRAKE) {
+                motor.brake();
+            } else if (zeroPowerBehavior == DcMotor.ZeroPowerBehavior.FLOAT) {
+                motor.coast();
+            }
         }
     }
 
@@ -233,7 +237,7 @@ public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coeffi
             coefficients.d,
             (coefficients.f * 12) / batteryVoltageSensor.getVoltage()
         );
-        for (DcMotorEx motor : motors) {
+        for (EncodedMotor<DcMotorEx> motor : motors) {
             motor.setPIDFCoefficients(runMode, compensatedCoefficients);
         }
     }
@@ -255,13 +259,13 @@ public void setWeightedDrivePower(Pose2d drivePower) {
     @Override
     public List<Double> getWheelPositions() {
         double leftSum = 0, rightSum = 0;
-        for (DcMotorEx leftMotor : leftMotors) {
+        for (EncodedMotor<DcMotorEx> leftMotor : leftMotors) {
             leftSum +=
-            TankConstants.encoderTicksToInches(leftMotor.getCurrentPosition(), WHEEL_RADIUS, GEAR_RATIO, TICKS_PER_REV);
-            for (DcMotorEx rightMotor : rightMotors) {
+            TankConstants.encoderTicksToInches(leftMotor.getSensorValue(), WHEEL_RADIUS, GEAR_RATIO, TICKS_PER_REV);
+            for (EncodedMotor<DcMotorEx> rightMotor : rightMotors) {
                 rightSum +=
                 TankConstants.encoderTicksToInches(
-                    rightMotor.getCurrentPosition(),
+                    rightMotor.getSensorValue(),
                     WHEEL_RADIUS,
                     GEAR_RATIO,
                     TICKS_PER_REV
@@ -273,11 +277,11 @@ public List<Double> getWheelPositions() {
 
     public List<Double> getWheelVelocities() {
         double leftSum = 0, rightSum = 0;
-        for (DcMotorEx leftMotor : leftMotors) {
+        for (EncodedMotor<DcMotorEx> leftMotor : leftMotors) {
             leftSum +=
             TankConstants.encoderTicksToInches(leftMotor.getVelocity(), WHEEL_RADIUS, GEAR_RATIO, TICKS_PER_REV);
         }
-        for (DcMotorEx rightMotor : rightMotors) {
+        for (EncodedMotor<DcMotorEx> rightMotor : rightMotors) {
             rightSum +=
             TankConstants.encoderTicksToInches(rightMotor.getVelocity(), WHEEL_RADIUS, GEAR_RATIO, TICKS_PER_REV);
         }
@@ -286,11 +290,11 @@ public List<Double> getWheelVelocities() {
 
     @Override
     public void setMotorPowers(double v, double v1) {
-        for (DcMotorEx leftMotor : leftMotors) {
-            leftMotor.setPower(v);
+        for (EncodedMotor<DcMotorEx> leftMotor : leftMotors) {
+            leftMotor.setSpeed(v);
         }
-        for (DcMotorEx rightMotor : rightMotors) {
-            rightMotor.setPower(v1);
+        for (EncodedMotor<DcMotorEx> rightMotor : rightMotors) {
+            rightMotor.setSpeed(v1);
         }
     }
 

Path/src/main/java/com/technototes/path/subsystem/TwoDeadWheelLocalizer.java

@@ -0,0 +1,104 @@
+package com.technototes.path.subsystem;
+
+import static com.technototes.path.subsystem.DeadWheelConstants.EncoderOverflow;
+import static com.technototes.path.subsystem.DeadWheelConstants.ForwardOffset;
+import static com.technototes.path.subsystem.DeadWheelConstants.GearRatio;
+import static com.technototes.path.subsystem.DeadWheelConstants.LateralDistance;
+import static com.technototes.path.subsystem.DeadWheelConstants.TicksPerRev;
+import static com.technototes.path.subsystem.DeadWheelConstants.WheelRadius;
+
+import androidx.annotation.NonNull;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer;
+import com.technototes.library.hardware.sensor.IMU;
+import com.technototes.library.hardware.sensor.encoder.MotorEncoder;
+import com.technototes.library.subsystem.Subsystem;
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.DoubleSupplier;
+
+/*
+ * Sample tracking wheel localizer implementation assuming a standard configuration:
+ *
+ *    /---------------\
+ *    |      ____     |
+ *    |      ----     |
+ *    | ||      ^     |
+ *    | ||<- lr |     |
+ *    |         fb    |
+ *    |               |
+ *    \---------------/
+ *
+ * COMPLETELY UNTESTED!!
+ */
+public class TwoDeadWheelLocalizer extends TwoTrackingWheelLocalizer implements Subsystem {
+
+    protected MotorEncoder leftrightEncoder, frontbackEncoder;
+    protected DoubleSupplier headingSupplier;
+    protected double lateralDistance, forwardOffset, gearRatio, wheelRadius, ticksPerRev;
+
+    protected boolean encoderOverflow;
+
+    public TwoDeadWheelLocalizer(IMU imu, MotorEncoder lr, MotorEncoder fb, DeadWheelConstants constants) {
+        super(
+            Arrays.asList(
+                new Pose2d(0, constants.getDouble(LateralDistance.class), 0), // left
+                new Pose2d(constants.getDouble(ForwardOffset.class), 0, Math.toRadians(90)) // front
+            )
+        );
+        leftrightEncoder = lr;
+        frontbackEncoder = fb;
+        headingSupplier = () -> imu.gyroHeading();
+
+        lateralDistance = constants.getDouble(LateralDistance.class);
+        forwardOffset = constants.getDouble(ForwardOffset.class);
+        encoderOverflow = constants.getBoolean(EncoderOverflow.class);
+        gearRatio = constants.getDouble(GearRatio.class);
+        ticksPerRev = constants.getDouble(TicksPerRev.class);
+        wheelRadius = constants.getDouble(WheelRadius.class);
+    }
+
+    public double encoderTicksToInches(double ticks) {
+        return ((getWheelRadius() * 2 * Math.PI * getGearRatio() * ticks) / getTicksPerRev());
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        return Arrays.asList(
+            encoderTicksToInches(leftrightEncoder.getCurrentPosition()),
+            encoderTicksToInches(frontbackEncoder.getCurrentPosition())
+        );
+    }
+
+    @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(leftrightEncoder.getCorrectedVelocity()),
+            encoderTicksToInches(frontbackEncoder.getCorrectedVelocity())
+        );
+    }
+
+    public double getTicksPerRev() {
+        return ticksPerRev;
+    }
+
+    public double getWheelRadius() {
+        return wheelRadius;
+    }
+
+    public double getGearRatio() {
+        return gearRatio;
+    }
+
+    @Override
+    public double getHeading() {
+        return headingSupplier.getAsDouble();
+    }
+}