quick rev down if we are going for a low speed

2491-NoMythic · Apr 28, 2024 · 6cbf980 · 6cbf980
1 parent ab413e5
commit 6cbf980
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 20 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -401,10 +401,9 @@ private void configureBindings() {
         new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter),
         new MoveMeters(driveTrain, 0.015, 0.5, 0, 0),
         new InstantCommand(driveTrain::pointWheelsInward, driveTrain),
-        new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem),
-        new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)),
+        new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem),
+        new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)),
         new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)),
-        new WaitCommand(0.08),
         new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer),
         new WaitCommand(0.5),
         new InstantCommand(()->intake.setNoteHeld(false))

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -41,8 +41,8 @@ public class ShooterSubsystem extends SubsystemBase {
 	 double m_DesiredShooterAngle;
    double targetVelocityL;
    double targetVelocityR;
-   boolean isRevingL;
-   boolean isRevingR;
+   int revStateL;
+   int revStateR;
 
   CurrentLimitsConfigs currentLimitConfigs;
    Slot0Configs PIDLeftconfigs; 
@@ -63,8 +63,8 @@ public class ShooterSubsystem extends SubsystemBase {
   /** Creates a new Shooter. */
   public ShooterSubsystem(double runSpeed) {
     if(Preferences.getBoolean("CompBot", true)) {
-      PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0);
-      PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0);
+      PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0.004);
+      PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0.004);
   } else {
       PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.PracRightkP).withKV(ShooterConstants.PracRightkFF).withKI(0);
       PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.PracLeftkP).withKV(ShooterConstants.PracLeftkFF).withKI(0);
@@ -197,25 +197,33 @@ public void turnOff(){
    * @param isReving Old isReving
    * @return Updated isReving
    */
-  private static boolean updateIsReving(boolean isReving, double targetSpeed, double speed){
-    double revUpEn =  -6;
+  private static int updateIsReving(int revState, double targetSpeed, double speed){
+    double revUpEn =  -10;
     double revUpDis = -2;
+    double revDownEn = 10;
+    double revDownDis = 2;
     targetSpeed = Math.abs(targetSpeed);
     speed = Math.abs(speed);
     if (speed < targetSpeed + revUpEn){
-      isReving = true;
-    } else if (speed > targetSpeed + revUpDis){
-      isReving = false;
-    }    
-    return isReving;
+      revState = 1;
+    } else if (speed > targetSpeed + revDownEn && targetSpeed<10){
+      revState = -1;
+    } else if((revState == 1 && speed > targetSpeed + revUpDis) || (revState == -1 && speed < targetSpeed + revDownDis)) {
+      revState = 0;
+    }
+    return revState;
   }
   /**
    * If the motor is Reving, set speed to full, if not Reving, use PID Mode
    */
-  private static void updateMotor(TalonFX shooterMotor, boolean revState, double targetSpeed){
+  private static void updateMotor(TalonFX shooterMotor, int revState, double targetSpeed, CurrentLimitsConfigs currentLimits){
+    currentLimits.StatorCurrentLimitEnable = revState != -1;
+    shooterMotor.getConfigurator().apply(currentLimits);
     if(targetSpeed == 0) {
       shooterMotor.set(0);
-    } else if (revState){
+    } else if (revState == -1) {
+      shooterMotor.set(-Math.signum(targetSpeed));
+    } else if(revState == 1) {
       shooterMotor.set(Math.signum(targetSpeed));
     } else {
       shooterMotor.setControl(new VelocityDutyCycle(targetSpeed).withSlot(0));
@@ -225,10 +233,10 @@ private static void updateMotor(TalonFX shooterMotor, boolean revState, double t
    *  Revs left and right motors while below target velocity, if above target velocity, the motors are put to PID Mode
    */
   private void updateMotors(){
-    isRevingL = updateIsReving(isRevingL, targetVelocityL, shooterL.getVelocity().getValueAsDouble());
-    isRevingR = updateIsReving(isRevingR, targetVelocityR, shooterR.getVelocity().getValueAsDouble());
-    updateMotor(shooterL, isRevingL, targetVelocityL); 
-    updateMotor(shooterR, isRevingR, targetVelocityR); 
+    revStateL = updateIsReving(revStateL, targetVelocityL, shooterL.getVelocity().getValueAsDouble());
+    revStateR = updateIsReving(revStateR, targetVelocityR, shooterR.getVelocity().getValueAsDouble());
+    updateMotor(shooterL, revStateL, targetVelocityL, currentLimitConfigs); 
+    updateMotor(shooterR, revStateR, targetVelocityR, currentLimitConfigs); 
   }
   public double getRSpeed() {
     return shooterR.getVelocity().getValueAsDouble();
@@ -249,6 +257,8 @@ public void periodic() {
     SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble());
     SmartDashboard.putNumber("Shooter/Right shooter speed", shooterR.getVelocity().getValueAsDouble());
     SmartDashboard.putNumber("Shooter/Left shooter speed", shooterL.getVelocity().getValueAsDouble());
+    SmartDashboard.putNumber("Shooter/Left shooter revState", revStateL);
+    SmartDashboard.putNumber("Shooter/right shooter revState", revStateR);
     SmartDashboard.putNumber("Shooter/right integral value", shooterR.getClosedLoopIntegratedOutput().getValueAsDouble());
     double error = getSignedError();
     RobotState.getInstance().ShooterError = error;