Working Shooter code with PID

Added PID again to shooter code
FRCTeam1967 · Mar 9, 2024 · 2b40aac · 2b40aac
1 parent 9bb1bf8
commit 2b40aac
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 18 deletions.
diff --git a/2024Crescendo/src/main/java/frc/robot/Constants.java b/2024Crescendo/src/main/java/frc/robot/Constants.java
@@ -121,13 +121,20 @@ public static class Shooter {
     public static final int TOP_LEFT_MOTOR_ID = 20, TOP_RIGHT_MOTOR_ID = 21;
     public static final int BOTTOM_LEFT_MOTOR_ID = 19, BOTTOM_RIGHT_MOTOR_ID = 22;
 
-    public static final double kP = 0.32, kI = 0, kD = 0;
+    public static final double kP = 0.32, kI = 0, kD = 0, kV = 0.07, kA = 0.06;
 
-    public static final double SPEAKER_TOP_VELOCITY = 1, SPEAKER_TOP_ACCELERATION = 1;
-    public static final double SPEAKER_BOTTOM_VELOCITY = 1, SPEAKER_BOTTOM_ACCELERATION = 1;
+    public static final double A_TOP_SPEED = 0.162;
+    public static final double A_BOTTOM_SPEED = 0.162;
 
-    public static final double AMP_TOP_VELOCITY = 0.08, AMP_TOP_ACCELERATION = 0.08;
-    public static final double AMP_BOTTOM_VELOCITY = 0.08, AMP_BOTTOM_ACCELERATION = 0.08;
+    public static final double AMP_TOP_VELOCITY = 12.5, AMP_TOP_ACCELERATION = 15;
+    public static final double AMP_BOTTOM_VELOCITY = 12.5, AMP_BOTTOM_ACCELERATION = 15;
+
+
+    public static final double S_TOP_SPEED = 1;
+    public static final double S_BOTTOM_SPEED = 1;
+
+    public static final double SPEAKER_TOP_VELOCITY = 100, SPEAKER_TOP_ACCELERATION = 90;
+    public static final double SPEAKER_BOTTOM_VELOCITY = 100, SPEAKER_BOTTOM_ACCELERATION = 90;
   }
 
   public static class Pivot {

diff --git a/2024Crescendo/src/main/java/frc/robot/commands/ShootSpeaker.java b/2024Crescendo/src/main/java/frc/robot/commands/ShootSpeaker.java
@@ -41,7 +41,7 @@ public void initialize() {
   @Override
   public void execute() {
     shooter.runShooter(topVelocity, topAcceleration, bottomVelocity, bottomAcceleration);
-    if (shooter.getVelocity() >= 90){
+    if (shooter.getAverageVelocity() >= 90){
       shooter.runShooter(topVelocity, topAcceleration, bottomVelocity, bottomAcceleration);
       feeder.feedFeeder(Constants.Feeder.FEED_SPEED);
     }

diff --git a/2024Crescendo/src/main/java/frc/robot/subsystems/Shooter.java b/2024Crescendo/src/main/java/frc/robot/subsystems/Shooter.java
@@ -8,6 +8,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.hardware.TalonFX;
 
@@ -35,15 +37,22 @@ public Shooter() {
    * @param config - configuration to use
    */
   private void configMotor(TalonFX motor, TalonFXConfiguration config) {
-    /*config.Slot0.kP = Constants.Shooter.kP;
+    config.Slot0.kP = Constants.Shooter.kP;
     config.Slot0.kI = Constants.Shooter.kI;
-    config.Slot0.kD = Constants.Shooter.kD;*/
+    config.Slot0.kD = Constants.Shooter.kD;
+    config.Slot0.kV = Constants.Shooter.kV;
+    config.Slot0.kA = Constants.Shooter.kA;
+    config.Slot0.kI = Constants.Shooter.kI;
+
+    /*config.MotionMagic.MotionMagicCruiseVelocity = 3; //rps (4)
+    config.MotionMagic.MotionMagicAcceleration = 10; //rps/s
+    config.MotionMagic.MotionMagicJerk = 0;*/
 
     motor.setNeutralMode(NeutralModeValue.Coast);
     motor.getConfigurator().apply(config);
 
     config.CurrentLimits.StatorCurrentLimitEnable = true;
-    config.CurrentLimits.StatorCurrentLimit = 80;
+    config.CurrentLimits.StatorCurrentLimit = 60;
   }
 
   /**
@@ -53,20 +62,29 @@ private void configMotor(TalonFX motor, TalonFXConfiguration config) {
    * @param bottomVelocity
    * @param bottomAcceleration
    */
+  public void runNoPID(double topSpeed, double bottomSpeed) {
+    topLeftMotor.set(-topSpeed);
+    topRightMotor.set(topSpeed);
+    bottomLeftMotor.set(-bottomSpeed);
+    bottomRightMotor.set(bottomSpeed);
+  }
+
   public void runShooter(double topVelocity, double topAcceleration, double bottomVelocity, double bottomAcceleration) {
-    /*topLeftMotor.setControl(new VelocityVoltage(-topVelocity, -topAcceleration, false, 0.0, 0, false, false, false));
+    topLeftMotor.setControl(new VelocityVoltage(-topVelocity, -topAcceleration, false, 0.0, 0, false, false, false));
     topRightMotor.setControl(new VelocityVoltage(topVelocity, topAcceleration, false, 0.0, 0, false, false, false));
     bottomLeftMotor
         .setControl(new VelocityVoltage(-bottomVelocity, -bottomAcceleration, false, 0.0, 0, false, false, false));
     bottomRightMotor
-        .setControl(new VelocityVoltage(bottomVelocity, bottomAcceleration, false, 0.0, 0, false, false, false));*/
-
-    topLeftMotor.set(-topVelocity);
-    topRightMotor.set(topAcceleration);
-    bottomLeftMotor.set(-bottomVelocity);
-    bottomRightMotor.set(bottomAcceleration);
+        .setControl(new VelocityVoltage(bottomVelocity, bottomAcceleration, false, 0.0, 0, false, false, false));
   }
 
+  /*public void runTopPID(double topVelocity, double topAcceleration, double bottomSpeed) {
+    topLeftMotor.setControl(new VelocityVoltage(-topVelocity, -topAcceleration, false, 0.0, 0, false, false, false));
+    topRightMotor.setControl(new VelocityVoltage(topVelocity, topAcceleration, false, 0.0, 0, false, false, false));
+    bottomLeftMotor.set(-bottomSpeed);
+    bottomRightMotor.set(bottomSpeed);
+  }*/
+
   /**
    * Stops all shooter motors
    */
@@ -83,14 +101,22 @@ public void stopMotors() {
   }
 
   public void configDashboard(ShuffleboardTab tab) {
-    tab.addDouble("Velocity", ()->getVelocity());
+    tab.addDouble("Average Velocity", ()->getAverageVelocity());
+    tab.addDouble("Top Left", ()->getMotorVelocity(topLeftMotor));
+    tab.addDouble("Top Right", ()->getMotorVelocity(topRightMotor));
+    tab.addDouble("Bottom Left", ()->getMotorVelocity(bottomLeftMotor));
+    tab.addDouble("Bottom Right", ()->getMotorVelocity(bottomRightMotor));
   }
 
-  public double getVelocity(){
+  public double getAverageVelocity(){
     double averageVelocity = (Math.abs(topLeftMotor.getVelocity().getValueAsDouble())+ Math.abs(topRightMotor.getVelocity().getValueAsDouble()) + Math.abs(bottomLeftMotor.getVelocity().getValueAsDouble()) + Math.abs(bottomRightMotor.getVelocity().getValueAsDouble()))/4.0;
     return averageVelocity; 
   }
 
+  public double getMotorVelocity(TalonFX motor){
+    return Math.abs(motor.getVelocity().getValueAsDouble()); 
+  }
+
   @Override
   public void periodic() {
   }