Merging most recent master branch

src/main/java/frc/robot/subsystems/Intake.java

@@ -9,8 +9,16 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.constants.IntakeConstants;
 
 public class Intake extends SubsystemBase{
@@ -23,6 +31,12 @@ public class Intake extends SubsystemBase{
 
     private ArmFeedforward intakeFeedForward;
 
+    private MutableMeasure<Voltage> appliedVoltage;
+    private MutableMeasure<Distance> distance;
+    private MutableMeasure<Velocity<Distance>> velocity;
+
+    private SysIdRoutine intakePivotRoutine;
+
     public Intake(){
         intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
         
@@ -52,6 +66,31 @@ public class Intake extends SubsystemBase{
             IntakeConstants.kDIntake
         );
 
+        appliedVoltage = MutableMeasure.mutable(Units.Volts.of(0));
+        distance = MutableMeasure.mutable(Units.Meters.of(0));
+        velocity = MutableMeasure.mutable(Units.MetersPerSecond.of(0));
+
+        intakePivotRoutine = new SysIdRoutine(
+            new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                    intakePivot.setVoltage(volts.in(Units.Volts));
+                }, 
+                (log) -> {
+                    log.motor("intakePivot")
+                        .voltage(appliedVoltage.mut_replace(
+                            intakePivot.get() * RobotController.getBatteryVoltage(), Units.Volts
+                        ))
+                        .linearPosition(
+                            distance.mut_replace(intakeEncoder.getPosition(), Units.Meters)
+                        )
+                        .linearVelocity(
+                            velocity.mut_replace(intakeEncoder.getVelocity(), Units.MetersPerSecond)
+                        );
+                }, 
+                this
+            )
+        );
     }
 
     public Command intakeDownCommand() {
@@ -109,6 +148,14 @@ public class Intake extends SubsystemBase{
         });
     }
 
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return intakePivotRoutine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return intakePivotRoutine.dynamic(direction);
+    }
+    
     public DoubleSupplier getIntakeAngle(){
         return intakeEncoder::getPosition;
     }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -11,13 +11,27 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.constants.ShooterConstants;
 
-public class Shooter extends SubsystemBase{
+public class Shooter extends SubsystemBase {
+    public enum ShooterSysIDMode {
+        PIVOT,
+        TOP,
+        BOTTOM
+    }
+
     private CANSparkMax topShooter;
     private CANSparkMax bottomShooter;
 
@@ -39,6 +53,14 @@ public class Shooter extends SubsystemBase{
 
     private DigitalInput shooterBeamBreak;
 
+    private MutableMeasure<Voltage> appliedVoltage;
+    private MutableMeasure<Distance> distance;
+    private MutableMeasure<Velocity<Distance>> velocity;
+
+    private SysIdRoutine shooterPivotRoutine;
+    private SysIdRoutine shooterTopRoutine;
+    private SysIdRoutine shooterBottomRoutine;
+
     public Shooter(){
         topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
         bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
@@ -88,6 +110,76 @@ public class Shooter extends SubsystemBase{
             ShooterConstants.kGShooterPivotFF, 
             ShooterConstants.kVShooterPivotFF
         );
+
+        appliedVoltage = MutableMeasure.mutable(Units.Volts.of(0));
+        distance = MutableMeasure.mutable(Units.Meters.of(0));
+        velocity = MutableMeasure.mutable(Units.MetersPerSecond.of(0));
+
+        shooterPivotRoutine = new SysIdRoutine(
+            new SysIdRoutine.Config(), 
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                    shooterPivot.setVoltage(volts.in(Units.Volts));
+                }, 
+                (log) -> {
+                    log.motor("shooterPivot")
+                        .voltage(appliedVoltage.mut_replace(
+                            shooterPivot.get() * RobotController.getBatteryVoltage(), Units.Volts
+                        ))
+                        .linearPosition(
+                            distance.mut_replace(pivotEncoder.getDistance(), Units.Meters)
+                        )
+                        .linearVelocity(
+                            velocity.mut_replace(pivotEncoder.getRate(), Units.MetersPerSecond)
+                        );
+                }, 
+                this
+            )
+        );
+
+        shooterTopRoutine = new SysIdRoutine(
+            new SysIdRoutine.Config(), 
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                    topShooter.setVoltage(volts.in(Units.Volts));
+                }, 
+                (log) -> {
+                    log.motor("shooterTop")
+                        .voltage(appliedVoltage.mut_replace(
+                            topShooter.get() * RobotController.getBatteryVoltage(), Units.Volts
+                        ))
+                        .linearPosition(
+                            distance.mut_replace(topEncoder.getPosition(), Units.Meters)
+                        )
+                        .linearVelocity(
+                            velocity.mut_replace(topEncoder.getVelocity(), Units.MetersPerSecond)
+                        );
+                }, 
+                this
+            )
+        );
+
+        shooterBottomRoutine = new SysIdRoutine(
+            new SysIdRoutine.Config(), 
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                    bottomShooter.setVoltage(volts.in(Units.Volts));
+                }, 
+                (log) -> {
+                    log.motor("shooterBottom")
+                        .voltage(appliedVoltage.mut_replace(
+                            bottomShooter.get() * RobotController.getBatteryVoltage(), Units.Volts
+                        ))
+                        .linearPosition(
+                            distance.mut_replace(bottomEncoder.getPosition(), Units.Meters)
+                        )
+                        .linearVelocity(
+                            velocity.mut_replace(bottomEncoder.getVelocity(), Units.MetersPerSecond)
+                        );
+                }, 
+                this
+            )
+        );
     }
 
     public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
@@ -145,4 +237,39 @@ public class Shooter extends SubsystemBase{
         });
     }
 
+    public Command sysIdQuasistatic(ShooterSysIDMode mode, SysIdRoutine.Direction direction) {
+        switch(mode) {
+            case PIVOT:
+                shooterPivotRoutine.quasistatic(direction);
+                break;
+            case TOP:
+                shooterTopRoutine.quasistatic(direction);
+                break;
+            case BOTTOM:
+            default:
+                shooterBottomRoutine.quasistatic(direction);
+                break;
+        }
+
+        //It'll never get here, because the switch above has a default case
+        return null;
+    }
+
+    public Command sysIdDynamic(ShooterSysIDMode mode, SysIdRoutine.Direction direction) {
+        switch(mode) {
+            case PIVOT:
+                shooterPivotRoutine.dynamic(direction);
+                break;
+            case TOP:
+                shooterTopRoutine.dynamic(direction);
+                break;
+            case BOTTOM:
+            default:
+                shooterBottomRoutine.dynamic(direction);
+                break;
+        }
+
+        //It'll never get here, because the switch above has a default case
+        return null;
+    }
 }