Drive holdheading encoder pid

src/com/nutrons/steamworks/Drivetrain.java

@@ -14,28 +14,36 @@
 import com.nutrons.framework.controllers.ControllerEvent;
 import com.nutrons.framework.controllers.Events;
 import com.nutrons.framework.controllers.LoopSpeedController;
-import com.nutrons.framework.util.FlowOperators;
+import com.nutrons.framework.subsystems.WpiSmartDashboard;
 import io.reactivex.Flowable;
 import io.reactivex.flowables.ConnectableFlowable;
 import io.reactivex.schedulers.Schedulers;
 import java.util.concurrent.TimeUnit;
 
 public class Drivetrain implements Subsystem {
-
   private static final double FEET_PER_WHEEL_ROTATION = 0.851;
   private static final double WHEEL_ROTATION_PER_ENCODER_ROTATION = 42.0 / 54.0;
   private static final double FEET_PER_ENCODER_ROTATION =
       FEET_PER_WHEEL_ROTATION * WHEEL_ROTATION_PER_ENCODER_ROTATION;
+  // PID for turning to an angle based on the gyro
+  private static final double ANGLE_P = 0.09;
+  private static final double ANGLE_I = 0.0;
+  private static final double ANGLE_D = 0.035;
+  private static final int ANGLE_BUFFER_LENGTH = 5;
+  // PID for distance driving based on encoders
+  private static final double DISTANCE_P = 0.2;
+  private static final double DISTANCE_I = 0.0;
+  private static final double DISTANCE_D = 0.0;
+  private static final int DISTANCE_BUFFER_LENGTH = 5;
+  // Time required to spend within the PID tolerance for the PID loop to terminate
+  private static final TimeUnit PID_TERMINATE_UNIT = TimeUnit.MILLISECONDS;
+  private static final long PID_TERMINATE_TIME = 1000;
   private final Flowable<Double> throttle;
   private final Flowable<Double> yaw;
   private final LoopSpeedController leftDrive;
   private final LoopSpeedController rightDrive;
-  private final double deadband = 0.3;
+  private static final double DEADBAND = 0.3;
   private final Flowable<Boolean> teleHoldHeading;
-  private final double angleP = 0.09;
-  private final double angleI = 0.0;
-  private final double angleD = 0.035;
-  private final int angleBufferLength = 5;
   private final ConnectableFlowable<Double> currentHeading;
 
   /**
@@ -52,13 +60,24 @@ public Drivetrain(Flowable<Boolean> teleHoldHeading, Flowable<Double> currentHea
                     LoopSpeedController leftDrive, LoopSpeedController rightDrive) {
     this.currentHeading = currentHeading.publish();
     this.currentHeading.connect();
-    this.throttle = throttle.map(deadbandMap(-deadband, deadband, 0.0)).onBackpressureDrop();
-    this.yaw = yaw.map(deadbandMap(-deadband, deadband, 0.0)).onBackpressureDrop();
+    this.throttle = throttle.map(deadbandMap(-DEADBAND, DEADBAND, 0.0)).onBackpressureDrop();
+    this.yaw = yaw.map(deadbandMap(-DEADBAND, DEADBAND, 0.0)).onBackpressureDrop();
     this.leftDrive = leftDrive;
     this.rightDrive = rightDrive;
     this.teleHoldHeading = teleHoldHeading;
   }
 
+  /**
+   * A stream which will emit an item once the error stream is within
+   * the tolerance for an acceptable amount of time, as determined by the constants.
+   * Intended use is to terminate a PID loop command.
+   */
+  private Flowable<?> pidTerminator(Flowable<Double> error, double tolerance) {
+    return error.map(x -> abs(x) < tolerance)
+        .distinctUntilChanged().debounce(PID_TERMINATE_TIME, PID_TERMINATE_UNIT)
+        .filter(x -> x);
+  }
+
   /**
    * A command that turns the Drivetrain by a given angle, stopping after the angle remains
    * within the specified tolerance for a certain period of time.
@@ -70,61 +89,86 @@ public Drivetrain(Flowable<Boolean> teleHoldHeading, Flowable<Double> currentHea
    */
   public Command turn(double angle, double tolerance) {
     return Command.just(x -> {
+      // Sets the targetHeading to the sum of one currentHeading value, with angle added to it.
       Flowable<Double> targetHeading = currentHeading.take(1).map(y -> y + angle);
       Flowable<Double> error = currentHeading.withLatestFrom(targetHeading, (y, z) -> y - z);
+      // driveHoldHeading, with 0.0 ideal left and right speed, to turn in place.
       Flowable<Terminator> terms = driveHoldHeading(Flowable.just(0.0), Flowable.just(0.0),
           Flowable.just(true), targetHeading)
+          // Makes sure the final terminator will stop the motors
           .addFinalTerminator(() -> {
             leftDrive.runAtPower(0);
             rightDrive.runAtPower(0);
-          }).terminable(error.map(y -> abs(y) < tolerance)
-              .distinctUntilChanged().debounce(500, TimeUnit.MILLISECONDS)
-              .filter(y -> y)).execute(x);
+          })
+          // Terminate when the error is below the tolerance for long enough
+          .terminable(pidTerminator(error, tolerance))
+          .execute(x);
       return terms;
+      // Ensure we do not spend too long attempting to turn
     }).endsWhen(Flowable.timer(1500, TimeUnit.MILLISECONDS), true);
   }
 
-  /**
-   * A command that will drive the robot forward for a given time.
-   *
-   * @param time the time to drive forwards for, in milliseconds
-   */
-  public Command driveTimeAction(long time) {
-    Flowable<Double> move = toFlow(() -> 0.4);
-    return Command.fromSubscription(() ->
-        combineDisposable(
-            move.map(x -> Events.power(x)).subscribe(leftDrive),
-            move.map(x -> Events.power(-x)).subscribe(rightDrive)
-        )
-    ).killAfter(time, TimeUnit.MILLISECONDS).then(Command.fromAction(() -> {
-      leftDrive.runAtPower(0);
-      rightDrive.runAtPower(0);
-    }));
-  }
-
   /**
    * Drive the robot until a certain distance is reached,
    * while using the gyro to hold the current heading.
    *
-   * @param distance the distance to drive forward in feet, (negative values drive backwards)
-   * @param speed    the controller's output speed
+   * @param distance          the distance to drive forward in feet,
+   *                          (negative values drive backwards)
+   * @param distanceTolerance the tolerance for distance error, which is based on encoder values;
+   *                          this error is based on encoder readings.
+   * @param angleTolerance    the tolerance for angle error in a sucessful PID loop;
+   *                          this error is based on gyro readings.
    */
-  public Command driveDistanceAction(double distance, double speed) {
-    System.out.println(FEET_PER_ENCODER_ROTATION);
+  public Command driveDistance(double distance,
+                               double distanceTolerance,
+                               double angleTolerance) {
+    // Get the current heading at the beginning
+    Flowable<Double> targetHeading = currentHeading.take(1).cache();
     ControllerEvent reset = Events.resetPosition(0);
     double setpoint = distance / FEET_PER_ENCODER_ROTATION;
-    System.out.println(setpoint);
-    Command resetRight = Command.just(x -> {
+
+    // Construct closed-loop streams for distance / encoder based PID
+    Flowable<Double> distanceError = toFlow(() ->
+        (rightDrive.position() + leftDrive.position()) / 2.0 - setpoint);
+    Flowable<Double> distanceOutput = distanceError
+        .compose(pidLoop(DISTANCE_P, DISTANCE_BUFFER_LENGTH, DISTANCE_I, DISTANCE_D))
+        .onBackpressureDrop();
+
+    // Construct closed-loop streams for angle / gyro based PID
+    Flowable<Double> angleError = combineLatest(targetHeading, currentHeading, (x, y) -> x - y)
+        .onBackpressureDrop();
+    Flowable<Double> angleOutput = pidAngle(targetHeading);
+    angleOutput.subscribe(new WpiSmartDashboard().getTextFieldDouble("angle"));
+    distanceOutput.subscribe(new WpiSmartDashboard().getTextFieldDouble("distance"));
+
+    // Create commands for each motor
+    Command right = Command.fromSubscription(() ->
+        combineLatest(distanceOutput, angleOutput, (x, y) -> x + y)
+            .map(limitWithin(-1.0, 1.0))
+            .map(Events::power).subscribe(rightDrive));
+    Command left = Command.fromSubscription(() ->
+        combineLatest(distanceOutput, angleOutput, (x, y) -> x - y)
+            .map(limitWithin(-1.0, 1.0))
+            .map(x -> -x)
+            .map(Events::power).subscribe(leftDrive));
+    Flowable<Double> noDrive = Flowable.just(0.0);
+
+    // Chaining all the commands together
+    return Command.parallel(Command.fromAction(() -> {
       rightDrive.accept(reset);
-      return Flowable.just(() -> {
-        rightDrive.runAtPower(0);
-        leftDrive.runAtPower(0);
-      });
-    });
-    Flowable<Double> drive = toFlow(() -> speed * Math.signum(distance));
-    return Command.parallel(resetRight,
-        driveHoldHeading(drive, drive, Flowable.just(true), currentHeading.take(1)))
-        .until(() -> (rightDrive.position() - setpoint) * Math.signum(distance) > 0.0);
+      leftDrive.accept(reset);
+    }), right, left)
+        // Terminates the distance PID when within acceptable error
+        .terminable(pidTerminator(distanceError,
+            distanceTolerance / WHEEL_ROTATION_PER_ENCODER_ROTATION))
+        // Turn to the targetHeading afterwards, and stop PID when within acceptable error
+        .then(driveHoldHeading(noDrive, noDrive, Flowable.just(true), targetHeading)
+            .terminable(pidTerminator(angleError, angleTolerance))
+            // Afterwards, stop the motors
+            .then(Command.fromAction(() -> {
+              leftDrive.runAtPower(0);
+              rightDrive.runAtPower(0);
+            })));
   }
 
   /**
@@ -139,23 +183,24 @@ public Command driveDistanceAction(double distance, double speed) {
   public Command driveHoldHeading(Flowable<Double> left, Flowable<Double> right,
                                   Flowable<Boolean> holdHeading, Flowable<Double> targetHeading) {
     return Command.fromSubscription(() -> {
-      Flowable<Double> output = combineLatest(targetHeading, currentHeading, (x, y) -> x - y)
-          .onBackpressureDrop().map(FlowOperators::printId)
-          .compose(pidLoop(angleP, angleBufferLength, angleI, angleD));
+      Flowable<Double> output = pidAngle(targetHeading);
       return combineDisposable(
           combineLatest(left, output, holdHeading, (x, o, h) -> x + (h ? o : 0.0))
+              .onBackpressureDrop()
               .subscribeOn(Schedulers.io())
               .onBackpressureDrop()
               .map(limitWithin(-1.0, 1.0))
               .map(Events::power)
               .subscribe(leftDrive),
           combineLatest(right, output, holdHeading, (x, o, h) -> x - (h ? o : 0.0))
+              .onBackpressureDrop()
               .subscribeOn(Schedulers.io())
               .onBackpressureDrop()
               .map(limitWithin(-1.0, 1.0))
               .map(x -> Events.power(-x))
               .subscribe(rightDrive));
     })
+        // Stop motors afterwards
         .addFinalTerminator(() -> {
           leftDrive.runAtPower(0);
           leftDrive.runAtPower(0);
@@ -177,6 +222,35 @@ public Command driveHoldHeading(Flowable<Double> left, Flowable<Double> right,
         holdHeading.filter(x -> x).withLatestFrom(currentHeading, (x, y) -> y)));
   }
 
+  /**
+   * Constructs an output stream for a PID closed loop based on the heading of the robot.
+   *
+   * @param targetHeading the heading which the system should achieve
+   */
+  private Flowable<Double> pidAngle(Flowable<Double> targetHeading) {
+    return combineLatest(targetHeading, currentHeading, (x, y) -> x - y)
+        .onBackpressureDrop()
+        .compose(pidLoop(ANGLE_P, ANGLE_BUFFER_LENGTH, ANGLE_I, ANGLE_D));
+  }
+
+  /**
+   * A command that will drive the robot forward for a given time.
+   *
+   * @param time the time to drive forwards for, in milliseconds
+   */
+  public Command driveTimeAction(long time) {
+    Flowable<Double> move = toFlow(() -> 0.4);
+    return Command.fromSubscription(() ->
+        combineDisposable(
+            move.map(Events::power).subscribe(leftDrive),
+            move.map(x -> -x).map(Events::power).subscribe(rightDrive)
+        )
+    ).killAfter(time, TimeUnit.MILLISECONDS).then(Command.fromAction(() -> {
+      leftDrive.runAtPower(0);
+      rightDrive.runAtPower(0);
+    }));
+  }
+
   /**
    * Drive the robot using the arcade-style joystick streams that were passed to the Drivetrain.
    * This is usually run during teleop.
@@ -190,6 +264,6 @@ public Command driveTeleop() {
 
   @Override
   public void registerSubscriptions() {
-    // intentionally empty
+    // Intentionally empty
   }
 }

src/com/nutrons/steamworks/RobotBootstrapper.java

@@ -16,7 +16,6 @@
 import com.nutrons.framework.subsystems.WpiSmartDashboard;
 import io.reactivex.Flowable;
 import io.reactivex.functions.Function;
-
 import java.util.concurrent.TimeUnit;
 
 public class RobotBootstrapper extends Robot {
@@ -55,8 +54,10 @@ static Function<Boolean, Flowable<Boolean>> delayTrue(long delay, TimeUnit unit)
 
   @Override
   public Command registerAuto() {
-    Command drive = this.drivetrain.driveDistanceAction(4.0, 0.3);
-    return drive.then(this.drivetrain.turn(-85, 1)).then(drive);
+    return Command.serial(
+        this.drivetrain.driveDistance(8.25, 0.25, 5),
+        this.drivetrain.turn(-85, 5),
+        this.drivetrain.driveDistance(2.5, 0.25, 5));
   }
 
   @Override

src/com/nutrons/steamworks/Turret.java

@@ -40,7 +40,7 @@ public Turret(Flowable<Double> angle, Flowable<String> state, Talon master,
 
   @Override
   public void registerSubscriptions() {
-    FlowOperators.deadband(joyControl).map(FlowOperators::printId).map(x -> Events.power(x / 4))
+    FlowOperators.deadband(joyControl).map(x -> Events.power(x / 4))
         .subscribe(hoodMaster); //TODO: remove this joystick
 
     /*this.fwdLim.map(b -> b ?