Splines fixes

CommandRobotBase.java

@@ -6,6 +6,7 @@
 import org.usfirst.frc4904.standard.humaninput.Operator;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 

commands/CancelCurrent.java

@@ -0,0 +1,32 @@
+package org.usfirst.frc4904.standard.commands;
+
+import org.usfirst.frc4904.standard.LogKitten;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This class cancels a command
+ */
+public class CancelCurrent extends CommandBase {
+	protected final SubsystemBase subsystem;
+
+	public CancelCurrent(SubsystemBase subsystem) {
+		super();
+		setName("Cancel " + subsystem.getName() + "'s current command.");
+		this.subsystem = subsystem;
+	}
+
+	public void initialize() {
+		LogKitten.v("Initializing " + getName());
+		if(subsystem.getCurrentCommand() != null){
+            subsystem.getCurrentCommand().cancel();
+        }
+	}
+
+	public boolean isFinished() {
+		return true;
+	}
+}

commands/RunFor.java

@@ -25,7 +25,6 @@ public RunFor(String name, CommandBase command, double duration) {
 		this.duration = duration;
 		this.command = command;
 		firstTick = true;
-		addRequirements((Subsystem[]) this.command.getRequirements().toArray());
 	}
 
 	/**

commands/RunIfElse.java

@@ -18,9 +18,6 @@ public RunIfElse(String name, CommandBase ifCommand, CommandBase elseCommand,
 		this.ifCommand = ifCommand;
 		this.elseCommand = elseCommand;
 		this.booleanSuppliers = booleanSuppliers;
-
-		addRequirements((Subsystem[]) ifCommand.getRequirements().toArray());
-		addRequirements((Subsystem[]) elseCommand.getRequirements().toArray());
 	}
 
 	public RunIfElse(CommandBase ifCommand, CommandBase elseCommand, BooleanSupplier... booleanSuppliers) {

commands/RunUntil.java

@@ -7,17 +7,18 @@
 public class RunUntil extends CommandBase {
 	protected final CommandBase command;
 	protected final Supplier<Boolean> stopCondition;
+	protected final boolean cancelOnEnd;
 
-	public RunUntil(String name, CommandBase command, Supplier<Boolean> stopCondition) {
+	public RunUntil(String name, CommandBase command, Supplier<Boolean> stopCondition, boolean cancelOnEnd) {
 		super();
 		setName(name);
 		this.command = command;
 		this.stopCondition = stopCondition;
-		addRequirements((Subsystem[]) command.getRequirements().toArray());
+		this.cancelOnEnd = cancelOnEnd;
 	}
 
 	public RunUntil(CommandBase command, Supplier<Boolean> stopCondition) {
-		this("RunUntil", command, stopCondition);
+		this("RunUntil", command, stopCondition, true);
 	}
 
 	@Override
@@ -32,6 +33,8 @@ public boolean isFinished() {
 
 	@Override
 	public void end(boolean interrupted) {
-		command.cancel();
+		if (cancelOnEnd){
+			command.cancel();
+		}
 	}
 }

commands/RunWhile.java

@@ -13,7 +13,6 @@ public RunWhile(String name, CommandBase command, Supplier<Boolean> stopConditio
 		setName(name);
 		this.command = command;
 		this.stopCondition = stopCondition;
-		addRequirements((Subsystem[]) command.getRequirements().toArray());
 	}
 
 	public RunWhile(CommandBase command, Supplier<Boolean> stopCondition) {

commands/chassis/SimpleSplines.java

@@ -2,98 +2,123 @@
 
 package org.usfirst.frc4904.standard.commands.chassis;
 
-import java.util.List;
-
-import org.usfirst.frc4904.robot.RobotMap;
 import org.usfirst.frc4904.standard.subsystems.chassis.SensorDrive;
+import org.usfirst.frc4904.standard.subsystems.chassis.SplinesDrive;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
+/**
+ * This command creates a simple spline to follow a Trajectory using a SensorDrive. Note that while the nextCommand to run after the robot finishes driving and the initialPos are both configurable, they will almost always be set automatically.
+ */
 public class SimpleSplines extends SequentialCommandGroup {
-  public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, List<Translation2d> inter_points, Pose2d final_pos, double maxVoltage, Command nextCommand){
-    super(new RamseteCommand(
-        TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos, new TrajectoryConfig(robotDrive.getAutoConstants().kMaxSpeedMetersPerSecond,
-        robotDrive.getAutoConstants().kMaxAccelerationMetersPerSecondSquared)
-            .setKinematics(robotDrive.getDriveConstants().kDriveKinematics)
-            .addConstraint(new DifferentialDriveVoltageConstraint(
-              new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, 
-              robotDrive.getDriveConstants().kvVoltSecondsPerMeter, 
-              robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), 
-              robotDrive.getDriveConstants().kDriveKinematics, 
-              maxVoltage))),
+  /**
+   * @param robotDrive the SensorDrive used to follow the trajectory.
+   * @param trajectory the Trajectory to follow.
+   * @param nextCommand the command to run immediately following the spline completion. In most cases, this should be setting the chassis voltages all to 0.
+   * @param initialPos the initial pose to reset the robot's odometry to.
+   */
+  public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory, Command nextCommand, Pose2d initialPos){
+    super(new InstantCommand(() -> robotDrive.resetOdometry(initialPos)),
+      new RamseteCommand(
+        trajectory,
         robotDrive::getPose,
-        new RamseteController(robotDrive.getAutoConstants().kRamseteB, robotDrive.getAutoConstants().kRamseteZeta),
-        new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts,
-        robotDrive.getDriveConstants().kvVoltSecondsPerMeter,
-        robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter),
-        robotDrive.getDriveConstants().kDriveKinematics,
+        new RamseteController(robotDrive.getAutoConstants().RAMSETE_B, robotDrive.getAutoConstants().RAMSETE_ZETA),
+        new SimpleMotorFeedforward(robotDrive.getDriveConstants().KS,
+        robotDrive.getDriveConstants().KV,
+        robotDrive.getDriveConstants().KA),
+        robotDrive.getDriveConstants().DRIVE_KINEMATICS,
         robotDrive::getWheelSpeeds,
-        new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0),
-        new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0),
-        // RamseteCommand passes volts to the callback
-        robotDrive::tankDriveVolts, robotDrive), nextCommand);
+        new PIDController(robotDrive.getDriveConstants().KP_DRIVE_VEL, 0, 0),
+        new PIDController(robotDrive.getDriveConstants().KP_DRIVE_VEL, 0, 0),
+        robotDrive::tankDriveVolts, robotDrive.getDriveBase().getMotors()), nextCommand);
+    addRequirements(robotDrive.getDriveBase());
   } 
 
-  public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, List<Translation2d> inter_points, Pose2d final_pos, double maxVoltage){
-    this(robotDrive, init_pos, inter_points, final_pos, maxVoltage, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0)));
+  /**
+   * @param robotDrive the SensorDrive used to follow the trajectory.
+   * @param trajectory the Trajectory to follow.
+   * @param initialPos the initial pose to reset the robot's odometry to.
+   */
+  public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory, Pose2d initialPos){
+    this(robotDrive, trajectory, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0)), initialPos);
   }
 
-  public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, List<Translation2d> inter_points, Pose2d final_pos){
-    this(robotDrive, init_pos, inter_points, final_pos, 10);
+  /**
+   * @param robotDrive the SensorDrive used to follow the trajectory.
+   * @param trajectory the Trajectory to follow.
+   * @param nextCommand the command to run immediately following the spline completion. In most cases, this should be setting the chassis voltages all to 0.
+   */
+  public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory, Command nextCommand){
+    this(robotDrive, trajectory, nextCommand, trajectory.getStates().get(0).poseMeters);
   }
 
-  public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, Pose2d final_pos){
-    this(robotDrive, init_pos, List.of(), final_pos);
+  /**
+   * @param robotDrive the SensorDrive used to follow the trajectory.
+   * @param trajectory the Trajectory to follow.
+   */
+  public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory){
+    this(robotDrive, trajectory, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0)));
   }
 
-
   /**
    * Class to store autonomous constants used for Ramsete Pathing.
    */
-  public static class AutoConstants {
-    public double kMaxSpeedMetersPerSecond;
-		public double kMaxAccelerationMetersPerSecondSquared;
-		public double kRamseteB;
-		public double kRamseteZeta;
-
-    public AutoConstants(double kMaxSpeedMetersPerSecond, double kMaxAccelerationMetersPerSecondSquared, double kRamseteB, double kRamseteZeta) {
-      this.kMaxSpeedMetersPerSecond = kMaxSpeedMetersPerSecond;
-      this.kMaxAccelerationMetersPerSecondSquared = kMaxAccelerationMetersPerSecondSquared;
-      this.kRamseteB = kRamseteB;
-      this.kRamseteZeta = kRamseteZeta;
+  public static class SplineAutoConstants {
+    public double MAX_SPEED;
+		public double MAX_ACCEL;
+		public double RAMSETE_B;
+    public double RAMSETE_ZETA;
+
+    /**
+     * Constructor for the auto constants.
+     * @param MAX_SPEED_MS max speed of the robot. Meters per second.
+     * @param MAX_ACCEL_MSS max acceleration of the robot. Meters per second squared.
+     * @param RAMSETE_B B coefficient. Should be 2.0 for most contexts.
+     * @param kRamseteZeta Zeta coefficient. Should be 0.7 for most contexts.
+     */
+    public SplineAutoConstants(double MAX_SPEED, double MAX_ACCEL, double RAMSETE_B, double RAMSETE_ZETA) {
+      this.MAX_SPEED = MAX_SPEED;
+      this.MAX_ACCEL = MAX_ACCEL;
+      this.RAMSETE_B = RAMSETE_B;
+      this.RAMSETE_ZETA = RAMSETE_ZETA;
     }
   }
 
   /**
    * Class to store drive constants used for Ramsete pathing.
    */
-  public static class DriveConstants {
-    public double ksVolts;
-		public double kvVoltSecondsPerMeter;
-    public double kaVoltSecondsSquaredPerMeter;
-    public double kTrackwidthMeters;
-		public DifferentialDriveKinematics kDriveKinematics;
-    public double kPDriveVel;
+  public static class SplineDriveConstants {
+    public double KS;
+		public double KV;
+    public double KA;
+    public double TRACK_WIDTH;
+		public DifferentialDriveKinematics DRIVE_KINEMATICS;
+    public double KP_DRIVE_VEL;
 
-    public DriveConstants(double ksVolts, double kvVoltSecondsPerMeter, double kaVoltSecondsSquaredPerMeter, double kTrackwidthMeters, double kPDriveVel){
-      this.ksVolts = ksVolts;
-      this.kvVoltSecondsPerMeter = kvVoltSecondsPerMeter;
-      this.kaVoltSecondsSquaredPerMeter = kaVoltSecondsSquaredPerMeter;
-      this.kTrackwidthMeters = kTrackwidthMeters;
-      this.kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters);
-      this.kPDriveVel = kPDriveVel;
+    /**
+     * Constructor for the drive constants.
+     * @param KS ksVolts (from characterization).
+     * @param KV kvVoltSecondsPerMeter (from characterization).
+     * @param KA kaVoltSecondsSquaredPerMeter (from characterization).
+     * @param TRACK_WIDTH kTrackwidthMeters (from characterization).
+     * @param KP_DRIVE_VEL kPDriveVel (from characterization).
+     */
+    public SplineDriveConstants(double KS, double KV, double KA, double TRACK_WIDTH, double KP_DRIVE_VEL){
+      this.KS = KS;
+      this.KV = KV;
+      this.KA = KA;
+      this.TRACK_WIDTH = TRACK_WIDTH;
+      this.DRIVE_KINEMATICS = new DifferentialDriveKinematics(TRACK_WIDTH);
+      this.KP_DRIVE_VEL = KP_DRIVE_VEL;
     }
   }
 }

commands/motor/MotorVelocitySet.java

@@ -1,5 +1,6 @@
 package org.usfirst.frc4904.standard.commands.motor;
 
+import org.usfirst.frc4904.robot.RobotMap;
 import org.usfirst.frc4904.standard.LogKitten;
 import org.usfirst.frc4904.standard.commands.Noop;
 import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException;
@@ -27,6 +28,7 @@ public MotorVelocitySet(String name, VelocitySensorMotor motor, double velocity,
 		super();
 		setName(name);
 		addRequirements(motor);
+		addRequirements(RobotMap.Component.flywheelMotorA, RobotMap.Component.flywheelMotorB); // TODO: @Daniel-E-B requirements should be done in the classes that extend this
 		this.motor = motor;
 		this.fallbackCommand = fallbackCommand;
 		this.setVelocity(velocity);
@@ -63,26 +65,26 @@ public void setVelocity(double velocity) {
 
 	@Override
 	public void initialize() {
-		try {
-			motor.reset();
+		// try { // TODO: @Daniel-E-B uncomment and test
+		// 	motor.reset();
 			motor.enableMotionController();
 			motor.set(velocity);
-		} catch (InvalidSensorException e) {
-			cancel();
-			fallbackCommand.schedule();
-		}
+		// } catch (InvalidSensorException e) {
+		// 	cancel();
+		// 	fallbackCommand.schedule();
+		// }
 	}
 
 	@Override
 	public void execute() {
 		motor.set(velocity);
-		Exception potentialSensorException = motor.checkSensorException();
-		if (potentialSensorException != null) {
-			cancel();
-			if (!fallbackCommand.isScheduled()) {
-				fallbackCommand.schedule();
-			}
-		}
+		// Exception potentialSensorException = motor.checkSensorException(); // TODO: @Daniel-E-B uncomment and test
+		// if (potentialSensorException != null) {
+		// 	cancel();
+		// 	if (!fallbackCommand.isScheduled()) {
+		// 		fallbackCommand.schedule();
+		// 	}
+		// }
 	}
 
 	@Override
@@ -92,6 +94,8 @@ public boolean isFinished() {
 
 	@Override
 	public void end(boolean interrupted) {
+		motor.disableMotionController();
+		motor.set(0.0);
 		if (!interrupted) {
 			motor.set(0.0);
 		}

custom/motioncontrollers/CustomPIDController.java

@@ -272,7 +272,7 @@ public double getError() {
 	 */
 	private double feedForward() {
 		if(sensor.getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement){
-			return F * Math.signum(setpoint);
+			return F * Math.signum(lastError);
 		} else {
 			return F * setpoint;
 		}

custom/sensors/CANTalonEncoder.java

@@ -184,4 +184,8 @@ public boolean getStoppedSafely() {
 	public double getRateSafely() {
 		return getRate();
 	}
+
+	public BaseTalon getTalon() {
+		return talon;
+	}
 }