Reset drivetrain modules when override or timeout

Author: kylecorry31

build.gradle

@@ -9,7 +9,7 @@ targetCompatibility = JavaVersion.VERSION_11
 def includeDesktopSupport = true
 
 group 'com.thegongoliers'
-version '6.1.1'
+version '6.1.2'
 
 
 publishing {

src/main/java/com/thegongoliers/output/drivetrain/AnchorModule.java

@@ -2,11 +2,12 @@
 
 import com.kylecorry.pid.PID;
 import com.thegongoliers.input.odometry.DistanceSensor;
+import com.thegongoliers.utils.Resettable;
 
 /**
  * A drivetrain module which will lock the drivetrain in place while a trigger condition is met
  */
-public class AnchorModule implements DriveModule {
+public class AnchorModule implements DriveModule, Resettable {
 
     private DistanceSensor mLeftDistanceSupplier, mRightDistanceSupplier;
     private PID mLeftPID, mRightPID;
@@ -88,4 +89,9 @@ public void setPID(PID pid) {
         mLeftPID = new PID(pid.getP(), pid.getI(), pid.getD());
         mRightPID = new PID(pid.getP(), pid.getI(), pid.getD());
     }
+
+    @Override
+    public void reset() {
+        stopHoldingPosition();
+    }
 }

src/main/java/com/thegongoliers/output/drivetrain/DriveSpeed.java

@@ -2,6 +2,8 @@
 
 import com.thegongoliers.math.GMath;
 
+import java.util.Objects;
+
 /**
  * A class representing the speed of a differential drivetrain
  */
@@ -85,4 +87,16 @@ public DriveSpeed plus(DriveSpeed other){
       return new DriveSpeed(GMath.clamp(l, -1, 1), GMath.clamp(r, -1, 1));
     }
 
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        DriveSpeed that = (DriveSpeed) o;
+        return Double.compare(that.left, left) == 0 && Double.compare(that.right, right) == 0;
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(left, right);
+    }
 }

src/main/java/com/thegongoliers/output/drivetrain/ModularDrivetrain.java

@@ -9,18 +9,20 @@
 import com.thegongoliers.input.time.RobotClock;
 import com.thegongoliers.output.interfaces.Drivetrain;
 
+import com.thegongoliers.utils.Resettable;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 
 /**
  * A wrapper class for a drivetrain which adds support for drive modules. Does not apply drive modules during tank driving.
  */
-public class ModularDrivetrain implements Drivetrain {
+public class ModularDrivetrain implements Drivetrain, Resettable {
 
     private Drivetrain drivetrain;
     private List<DriveModule> modules;
     private DriveSpeed currentSpeed;
     private Clock clock;
     private double lastTime;
+    private double resetThreshold = Double.POSITIVE_INFINITY;
 
     /**
      * Default constructor
@@ -74,6 +76,10 @@ public void tank(double leftSpeed, double rightSpeed) {
         double time = clock.getTime();
         double dt = time - lastTime;
 
+        if (dt >= resetThreshold){
+            reset();
+        }
+
         var overrides = modules.stream().filter(DriveModule::overridesUser).collect(Collectors.toList());
         DriveModule override = null;
         if (!overrides.isEmpty()) {
@@ -94,8 +100,8 @@ public void tank(double leftSpeed, double rightSpeed) {
             if (override == null) {
                 desiredSpeed = module.run(currentSpeed, desiredSpeed, dt);
             } else {
-                // Run the module to ensure it stay updated, but don't update the desired speed since it should be overridden
-                module.run(currentSpeed, desiredSpeed, dt);
+                // Reset the modules not being run
+                resetModule(module);
             }
         }
 
@@ -133,6 +139,16 @@ public void removeModule(DriveModule module) {
         modules.remove(module);
     }
 
+    /**
+     * Reset a module
+     * @param module the module to reset
+     */
+    public void resetModule(DriveModule module){
+        if (module instanceof Resettable){
+            ((Resettable)module).reset();
+        }
+    }
+
     /**
      * Get all the installed modules
      *
@@ -159,4 +175,16 @@ public <T> T getInstalledModule(Class<T> cls) {
     }
 
 
+    @Override
+    public void reset() {
+        modules.forEach(this::resetModule);
+    }
+
+    /**
+     * If a drive method was not called within the reset threshold, all modules will be reset on the next invocation.
+     * @param resetThreshold The reset threshold in seconds.
+     */
+    public void setInactiveResetSeconds(double resetThreshold){
+        this.resetThreshold = resetThreshold;
+    }
 }

src/main/java/com/thegongoliers/output/drivetrain/PathFollowerModule.java

@@ -12,14 +12,15 @@
 import com.thegongoliers.paths.PathStep;
 import com.thegongoliers.paths.PathStepType;
 
+import com.thegongoliers.utils.Resettable;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 
 /**
  * A drivetrain module which will follow a path when activated
  */
 @UsedInCompetition(team = "5112", year = "2020")
-public class PathFollowerModule implements DriveModule {
+public class PathFollowerModule implements DriveModule, Resettable {
 
     private static final double DEFAULT_FORWARD_TOLERANCE = 0.1;
     private static final double DEFAULT_TURN_TOLERANCE = 0.1;
@@ -35,6 +36,9 @@ public class PathFollowerModule implements DriveModule {
     private SimplePath currentPath;
     private int currentStepIdx;
 
+    private double distanceZero;
+    private double angleZero;
+
 
     public PathFollowerModule(Gyro gyro, List<EncoderSensor> encoders, double forwardStrength, double turnStrength){
         this(gyro, encoders, new PID(forwardStrength, 0, 0), new PID(turnStrength, 0, 0));
@@ -185,7 +189,7 @@ private DriveSpeed getToNextStep() {
     }
 
     private DriveSpeed rotateTowards(double angle){
-        return DriveSpeed.fromArcade(0, mTurnPID.calculate(mGyro.getAngle(), angle));
+        return DriveSpeed.fromArcade(0, mTurnPID.calculate(getAngle(), angle));
     }
 
     private DriveSpeed driveTowards(double distance){
@@ -202,11 +206,20 @@ private boolean isDoneDriving(){
     }
 
     private double getDistance(){
-        return mEncoder.getDistance();
+        return mEncoder.getDistance() - distanceZero;
+    }
+
+    private double getAngle(){
+        return mGyro.getAngle() - angleZero;
     }
 
     private void zeroSensors(){
-        mEncoder.reset();
-        mGyro.reset();
+        distanceZero = mEncoder.getDistance();
+        angleZero = mGyro.getAngle();
+    }
+
+    @Override
+    public void reset() {
+        stopFollowingPath();
     }
 }

src/main/java/com/thegongoliers/output/drivetrain/StabilityModule.java

@@ -3,12 +3,13 @@
 import com.thegongoliers.input.time.Clock;
 import com.thegongoliers.input.time.RobotClock;
 
+import com.thegongoliers.utils.Resettable;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 
 /**
  * A drivetrain module which will stabilize the drivetrain (rotation-wise) while attempting to drive straight
  */
-public class StabilityModule implements DriveModule {
+public class StabilityModule implements DriveModule, Resettable {
 
     private static final double DEFAULT_TURN_THRESHOLD = 0.01;
 
@@ -28,7 +29,6 @@ public class StabilityModule implements DriveModule {
      * @param settlingTime the amount of time to allow the drivetrain to settle after turn inputs stop before applying turn corrections
      */
     public StabilityModule(Gyro gyro, double strength, double settlingTime){
-        super();
         setGyro(gyro);
         setStrength(strength);
         setSettlingTime(settlingTime);
@@ -81,6 +81,7 @@ public void setStrength(double strength){
     /**
      * Resets the desired heading of the robot to match the gyro
      **/
+    @Override
     public void reset() {
         updateDesiredHeading();   
     }

src/main/java/com/thegongoliers/output/drivetrain/TargetAlignmentModule.java

@@ -3,11 +3,12 @@
 import com.kylecorry.pid.PID;
 import com.thegongoliers.input.vision.TargetingCamera;
 import com.thegongoliers.math.GMath;
+import com.thegongoliers.utils.Resettable;
 
 /**
  * A drivetrain module which align to vision targets
  */
-public class TargetAlignmentModule implements DriveModule {
+public class TargetAlignmentModule implements DriveModule, Resettable {
 
     private static final double DEFAULT_RANGE_THRESHOLD = 0.1;
     private static final double DEFAULT_AIM_THRESHOLD = 0.1;
@@ -162,4 +163,9 @@ private boolean shouldOnlyAlignAim() {
             GMath.approximately(mRangePID.getI(), 0) && 
             GMath.approximately(mRangePID.getD(), 0);
     }
+
+    @Override
+    public void reset() {
+        stopAligning();
+    }
 }