Minor formatting change

src/main/java/frc/robot/RobotContainer.java

@@ -132,26 +132,6 @@ public class RobotContainer {
             )
         );
 
-        //intake.setDefaultCommand(intake.intakeUpCommand());
-        intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, driver::getLeftTriggerAxis));
-
-       // shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
-        shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
-
-        climber.setDefaultCommand(climber.stop());
-
-        indexer.setDefaultCommand(indexer.shootNote(operator::getRightTriggerAxis));
-
-        driver.povCenter().onFalse(
-            drivetrain.driveCardinal(
-                driver::getLeftY, 
-                driver::getLeftX, 
-                driver.getHID()::getPOV,
-                OIConstants.kTeleopDriveDeadband).until(
-                () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-                )
-        );
-
         // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
         driver.a().onTrue(
             drivetrain.driveAprilTagLock(
@@ -188,6 +168,17 @@ public class RobotContainer {
             )
         );
 
+        driver.povCenter().onFalse(
+            drivetrain.driveCardinal(
+                driver::getLeftY, 
+                driver::getLeftX, 
+                driver.getHID()::getPOV,
+                OIConstants.kTeleopDriveDeadband
+            ).until(
+                    () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+            )
+        );
+
         // This was originally a run while held, not sure that's really necessary, change it if need be
         driver.y().onTrue(drivetrain.zeroHeadingCommand());
 
@@ -205,9 +196,48 @@ public class RobotContainer {
         */
         driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
 
-        driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
+        //intake.setDefaultCommand(intake.intakeUpCommand());
+        //Intake rollers intentionally disabled at the moment
+        intake.setDefaultCommand(
+            intake.manualPivot(
+                () -> { return operator.getRightY() / 2; }, 
+                () -> { return 0; }
+            )
+        );
 
-        operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
+       // shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
+        shooter.setDefaultCommand(
+            shooter.manualPivot(
+                () -> { return operator.getLeftY() / 2; }, 
+                () -> {
+                    if (MathUtil.applyDeadband(driver.getLeftTriggerAxis(), OIConstants.kTeleopDriveDeadband) != 0) {
+                        return driver.getLeftTriggerAxis();
+                    } else {
+                        return -operator.getLeftTriggerAxis();
+                    }
+                }
+            )
+        );
+
+        indexer.setDefaultCommand(
+            indexer.shootNote(
+                () -> {
+                    if (MathUtil.applyDeadband(driver.getLeftTriggerAxis(), OIConstants.kTeleopDriveDeadband) != 0) {
+                        return driver.getLeftTriggerAxis();
+                    } else {
+                        return -operator.getLeftTriggerAxis();
+                    }
+                }
+            )
+        );
+
+        climber.setDefaultCommand(climber.stop());
+
+        operator.rightBumper().whileTrue(
+            climber.setSpeedWithSupplier(
+                operator::getRightTriggerAxis
+            )
+        );
         
   }
 

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

@@ -9,16 +9,8 @@ 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{
@@ -31,12 +23,6 @@ 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);
         
@@ -66,31 +52,6 @@ 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() {
@@ -148,14 +109,6 @@ 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,27 +11,13 @@ 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 enum ShooterSysIDMode {
-        PIVOT,
-        TOP,
-        BOTTOM
-    }
-
+public class Shooter extends SubsystemBase{
     private CANSparkMax topShooter;
     private CANSparkMax bottomShooter;
 
@@ -53,14 +39,6 @@ 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);
@@ -110,76 +88,6 @@ 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){
@@ -237,39 +145,4 @@ 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;
-    }
 }