Improve Driver Controls

src/main/java/com/team6962/lib/swerve/config/XBoxTeleopSwerveConstants.java

@@ -28,7 +28,7 @@ public class XBoxTeleopSwerveConstants implements Cloneable {
    * facing directly away from the driver station. This constant can be set to null to make no
    * button zero the robot's yaw.
    */
-  public Button ZeroYawButton = Button.kLeftBumper;
+  public Button ZeroYawButton = Button.kY;
 
   /** When held, this button will enable the boost speeds for translation and rotation. */
   public Trigger BoostAxis = Trigger.RightTrigger;

src/main/java/frc/robot/auto/shoot/AutoShoot.java

@@ -103,6 +103,7 @@ public class AutoShoot extends Command {
 
   private Angle hoodOffset = Degrees.of(0);
   private Angle turretOffset = Degrees.of(0);
+  private AngularVelocity flywheelSpeedOffset = RotationsPerSecond.of(0);
 
   /**
    * Creates a new AutoShoot command, which automatically aims and spins up the shooter rollers to
@@ -251,6 +252,11 @@ public Angle setTurretOffset(Angle offset) {
     return turretOffset;
   }
 
+  public AngularVelocity setFlywheelSpeedOffset(AngularVelocity offset) {
+    this.flywheelSpeedOffset = offset;
+    return flywheelSpeedOffset;
+  }
+
   @Override
   public void initialize() {
     readyToShoot = false;
@@ -478,6 +484,10 @@ public void execute() {
 
     DogLog.log("AutoShoot/HoodOffset", hoodOffset.in(Degrees), Degrees);
     DogLog.log("AutoShoot/TurretOffset", turretOffset.in(Degrees), Degrees);
+    DogLog.log(
+        "AutoShoot/FlywheelSpeedOffset",
+        flywheelSpeedOffset.in(RotationsPerSecond),
+        RotationsPerSecond);
 
     // Calculate the ideal shooting angles and roller speed to hit the target
     ShootingParameters appliedShootingParameters = calculate(Seconds.of(predictionTime));
@@ -492,7 +502,7 @@ public void execute() {
     // Set the target angles and roller speed
     turretAngleTarget = appliedShootingParameters.turretAngle.plus(turretOffset);
     hoodAngleTarget = appliedShootingParameters.hoodAngle.plus(hoodOffset);
-    rollerSpeedTarget = appliedShootingParameters.rollerSpeed;
+    rollerSpeedTarget = appliedShootingParameters.rollerSpeed.plus(flywheelSpeedOffset);
 
     if (previousTurretAngleTarget != null && previousHoodAngleTarget != null) {
       double timeSinceLastPeriodic = Timer.getFPGATimestamp() - previousPeriodicTimestamp;

src/main/java/frc/robot/controls/TeleopControls.java

@@ -174,10 +174,7 @@ public void configureBindings() {
             Commands.parallel(
                 this.robot.getIntakeRollers().outtake(), this.robot.getHopper().dump()));
 
-    // Intake and drive to fuel clump
-    // driver.start().whileTrue(driveToClump.driveToClump());
-
-    // Intake without driving - WORKS
+    // Intake - WORKS
     driver
         .rightStick()
         .and(RobotState::isEnabled)
@@ -205,7 +202,7 @@ public void configureBindings() {
                 robot.getShooterRollers().shoot(RotationsPerSecond.of(22.5))));
 
     // Unjam hopper - WORKS
-    operator.leftBumper().whileTrue(robot.getHopper().unjam());
+    operator.leftBumper().or(driver.leftBumper()).whileTrue(robot.getHopper().unjam());
 
     // Backup shoot
     operator.leftTrigger().and(RobotState::isEnabled).whileTrue(robot.getHopper().feed());
@@ -296,7 +293,7 @@ public void configureBindings() {
                     .moveAtVoltage(IntakeExtensionConstants.FINE_CONTROL_VOLTAGE.unaryMinus())));
 
     // Intake extension and retraction - WORKS
-    Trigger intakeRetract = operator.rightStick();
+    Trigger intakeRetract = operator.rightStick().or(driver.start());
     Trigger intakeExtend =
         intakeRetract.negate().and(RobotState::isTeleop).and(RobotState::isEnabled);
 
@@ -456,9 +453,11 @@ public void periodic() {
     if (!fineControl) {
       autoShoot.setHoodOffset(mapOffset(operator.getLeftY(), Degrees.of(5)));
       autoShoot.setTurretOffset(mapOffset(-operator.getLeftX(), Degrees.of(10)));
+      autoShoot.setFlywheelSpeedOffset(mapOffset(-operator.getRightY(), RotationsPerSecond.of(1)));
     } else {
       autoShoot.setHoodOffset(Degrees.of(0));
       autoShoot.setTurretOffset(Degrees.of(0));
+      autoShoot.setFlywheelSpeedOffset(RotationsPerSecond.of(0));
     }
 
     ControllerLogging.logInputs(driver.getHID());
@@ -476,9 +475,21 @@ private Angle mapOffset(double joystickInput, Angle maxOffset) {
       joystickInput /= 0.9;
 
       return Degrees.of(
+          Math.signum(joystickInput) * (joystickInput * joystickInput) * maxOffset.in(Degrees));
+    }
+  }
+
+  private AngularVelocity mapOffset(double joystickInput, AngularVelocity maxOffset) {
+    if (Math.abs(joystickInput) < 0.1) {
+      return RotationsPerSecond.of(0);
+    } else {
+      joystickInput -= Math.signum(joystickInput) * 0.1;
+      joystickInput /= 0.9;
+
+      return RotationsPerSecond.of(
           Math.signum(joystickInput)
-              * Math.pow(Math.abs(joystickInput), 2)
-              * maxOffset.in(Degrees));
+              * (joystickInput * joystickInput)
+              * maxOffset.in(RotationsPerSecond));
     }
   }
 }