Merge branch 'development' of https://github.com/awtybots/FRC-2024 in…

…to development
awtybots · Feb 29, 2024 · 30bfa42 · 30bfa42
2 parents 436d149 + ca2b4b4
commit 30bfa42
Show file tree

Hide file tree

Showing 4 changed files with 52 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -67,14 +67,14 @@ public static final class FlywheelConstants {
     public static final double armConversion = 1;
 
     // Flywheel PID constants
-    public static final double kP = 0.0004;
+    public static final double kP = 0.03;
     public static final double kI = 0.0;
-    public static final double kD = 0;
+    public static final double kD = 0.001;
 
     // Flywheel Feedforward characterization constants
     public static final double ks = 0;
     public static final double kv = 0;
-    public static final double shootingVelocity = 1000;
+    public static final double shootingVelocity = 5000; // revolutions per second
   }
 
   public static final class IntakeConstants {

diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java
@@ -42,10 +42,8 @@ public static Command intakeShooterDrive(
           if (rightBumperSupplier.getAsBoolean()) {
             flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity);
 
-            double flywheelRPM = flywheel.getVelocityRPMBottom();
-            double targetRPM =
-                Constants.FlywheelConstants.shootingVelocity
-                    + 2000; // TODO find actual max velocity
+            double flywheelRPM = -flywheel.getVelocityRPMBottom();
+            double targetRPM = Constants.FlywheelConstants.shootingVelocity * 0.9;
 
             if (flywheelRPM > targetRPM) {
               intake.runPercentSpeed(1);

diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java
@@ -55,7 +55,7 @@ public void periodic() {
 
   /** Run open loop at the specified voltage. */
   public void runVolts(double volts) {
-    io.setVoltage(volts);
+    // io.setVoltage(volts);
   }
 
   /** Run closed loop at the specified velocity. */

diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java
@@ -18,6 +18,8 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.Constants;
 import frc.robot.Constants.FlywheelConstants;
 import org.littletonrobotics.junction.AutoLogOutput;
 
@@ -38,6 +40,8 @@ will need to be treated differently (because if they were, they could rip the no
       new CANSparkMax(FlywheelConstants.kBottomFlywheelSparkMaxCanId, MotorType.kBrushless);
   private final RelativeEncoder bottomShooterEncoder = bottomShooterMotor.getEncoder();
   private final SparkPIDController bottomShooterPID = bottomShooterMotor.getPIDController();
+  private final PIDController topMotorPID;
+  private final PIDController bottomMotorPID;
 
   private double targetVelocity;
 
@@ -53,26 +57,37 @@ public FlywheelIOSparkMax() {
 
     topShooterMotor.setSmartCurrentLimit(30);
     bottomShooterMotor.setSmartCurrentLimit(30);
+
+    topMotorPID =
+        new PIDController(
+            Constants.FlywheelConstants.kP,
+            Constants.FlywheelConstants.kI,
+            Constants.FlywheelConstants.kD);
+    bottomMotorPID =
+        new PIDController(
+            Constants.FlywheelConstants.kP,
+            Constants.FlywheelConstants.kI,
+            Constants.FlywheelConstants.kD);
   }
 
   @Override
   public void updateInputs(FlywheelIOInputs inputs) {
-    inputs.positionRadTop = topShooterEncoder.getPosition() / GEAR_RATIO;
-    inputs.velocityRadPerSecTop = topShooterEncoder.getVelocity() / GEAR_RATIO;
+    inputs.positionRadTop = topShooterEncoder.getPosition();
+    inputs.velocityRadPerSecTop = topShooterEncoder.getVelocity();
     inputs.appliedVoltsTop = topShooterMotor.getAppliedOutput() * topShooterMotor.getBusVoltage();
     inputs.currentAmpsTop = new double[] {topShooterMotor.getOutputCurrent()};
 
     inputs.positionRadBottom = bottomShooterEncoder.getPosition() / GEAR_RATIO;
-    inputs.velocityRadPerSecBottom = bottomShooterEncoder.getVelocity() / GEAR_RATIO;
+    inputs.velocityRadPerSecBottom = bottomShooterEncoder.getVelocity();
     inputs.appliedVoltsBottom =
         bottomShooterMotor.getAppliedOutput() * bottomShooterMotor.getBusVoltage();
     inputs.currentAmpsBottom = new double[] {bottomShooterMotor.getOutputCurrent()};
   }
 
   @Override
   public void setVoltage(double volts) {
-    topShooterMotor.setVoltage(volts);
-    bottomShooterMotor.setVoltage(volts);
+    // topShooterMotor.setVoltage(volts);
+    // bottomShooterMotor.setVoltage(volts);
   }
 
   @AutoLogOutput(key = "Flywheel/targetVelocity")
@@ -85,11 +100,28 @@ public double getTopVelocity() {
     return topShooterEncoder.getVelocity();
   }
 
+  @AutoLogOutput(key = "Flywheel/BottomVelocity")
+  public double getBottomVelocity() {
+    return bottomShooterEncoder.getVelocity();
+  }
+
   @Override
-  public void setVelocity(double velocityRevPerSec, double ffVolts) {
-    targetVelocity = velocityRevPerSec * GEAR_RATIO;
+  public void setVelocity(double velocityRevPerSec, double ffVolts) { // DO NOT TRUST UNITS
+    targetVelocity = velocityRevPerSec;
     topShooterPID.setReference(targetVelocity, ControlType.kVelocity, 0, ffVolts);
     bottomShooterPID.setReference(targetVelocity, ControlType.kVelocity, 0, ffVolts);
+    double topShooterVelocity = topShooterEncoder.getVelocity();
+    double bottomShooterVelocity = topShooterEncoder.getVelocity();
+
+    topShooterMotor.set(
+        topMotorPID.calculate(topShooterVelocity, velocityRevPerSec)
+            + Constants.FlywheelConstants.kv * velocityRevPerSec
+            + Constants.FlywheelConstants.ks);
+
+    bottomShooterMotor.set(
+        bottomMotorPID.calculate(bottomShooterVelocity, velocityRevPerSec)
+            + Constants.FlywheelConstants.kv * velocityRevPerSec
+            + Constants.FlywheelConstants.ks);
   }
 
   @Override
@@ -100,14 +132,12 @@ public void stop() {
 
   @Override
   public void configurePID(double kP, double kI, double kD) {
-    topShooterPID.setP(kP, 0);
-    topShooterPID.setI(kI, 0);
-    topShooterPID.setD(kD, 0);
-    topShooterPID.setFF(0, 0);
-
-    bottomShooterPID.setP(kP, 0);
-    bottomShooterPID.setI(kI, 0);
-    bottomShooterPID.setD(kD, 0);
-    bottomShooterPID.setFF(0, 0);
+    topMotorPID.setP(kP);
+    topMotorPID.setI(kI);
+    topMotorPID.setD(kD);
+
+    bottomMotorPID.setP(kP);
+    bottomMotorPID.setI(kI);
+    bottomMotorPID.setD(kD);
   }
 }