diff --git a/src/com/nutrons/steamworks/Drivetrain.java b/src/com/nutrons/steamworks/Drivetrain.java index a7db389..d47223e 100644 --- a/src/com/nutrons/steamworks/Drivetrain.java +++ b/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 throttle; private final Flowable 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 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 currentHeading; /** @@ -52,13 +60,24 @@ public Drivetrain(Flowable teleHoldHeading, Flowable 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 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 teleHoldHeading, Flowable 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 targetHeading = currentHeading.take(1).map(y -> y + angle); Flowable error = currentHeading.withLatestFrom(targetHeading, (y, z) -> y - z); + // driveHoldHeading, with 0.0 ideal left and right speed, to turn in place. Flowable 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 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 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 distanceError = toFlow(() -> + (rightDrive.position() + leftDrive.position()) / 2.0 - setpoint); + Flowable distanceOutput = distanceError + .compose(pidLoop(DISTANCE_P, DISTANCE_BUFFER_LENGTH, DISTANCE_I, DISTANCE_D)) + .onBackpressureDrop(); + + // Construct closed-loop streams for angle / gyro based PID + Flowable angleError = combineLatest(targetHeading, currentHeading, (x, y) -> x - y) + .onBackpressureDrop(); + Flowable 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 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 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 left, Flowable right, Flowable holdHeading, Flowable targetHeading) { return Command.fromSubscription(() -> { - Flowable output = combineLatest(targetHeading, currentHeading, (x, y) -> x - y) - .onBackpressureDrop().map(FlowOperators::printId) - .compose(pidLoop(angleP, angleBufferLength, angleI, angleD)); + Flowable 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 left, Flowable 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 pidAngle(Flowable 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 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 } } diff --git a/src/com/nutrons/steamworks/RobotBootstrapper.java b/src/com/nutrons/steamworks/RobotBootstrapper.java index a1c9a5f..d1779e0 100644 --- a/src/com/nutrons/steamworks/RobotBootstrapper.java +++ b/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> 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 diff --git a/src/com/nutrons/steamworks/Turret.java b/src/com/nutrons/steamworks/Turret.java index 61ea424..5e2e5b7 100644 --- a/src/com/nutrons/steamworks/Turret.java +++ b/src/com/nutrons/steamworks/Turret.java @@ -40,7 +40,7 @@ public Turret(Flowable angle, Flowable 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 ?