Fixed Flywheel PID and FlywheelIONeo

team422 · Sep 2, 2024 · 5bf1146 · 5bf1146
1 parent 0d734ca
commit 5bf1146
Show file tree

Hide file tree

Showing 10 changed files with 120 additions and 130 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -103,19 +103,18 @@ public static final class ShooterConstants {
         new LoggedTunableNumber("ShooterIdleVoltage", 0.0);
     public static final LoggedTunableNumber kTopShootingVoltage =
         new LoggedTunableNumber("TopShooterShootingVoltage", 100000000000000000000000000001.0);
-    public static final LoggedTunableNumber kBottomShootingVoltage = new LoggedTunableNumber("BottomShooterShootingVoltage", 422);
+    public static final LoggedTunableNumber kBottomShootingVoltage =
+        new LoggedTunableNumber("BottomShooterShootingVoltage", 422);
     public static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel P", 40);
     public static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel I", 0.1);
     public static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel D", 0.1);
 
-
     // Simulation constants
     public static final double kSimRadius = Units.inchesToMeters(3);
     public static final double kSimMass = 0.85; // in kg
     public static final double kSimMOI = 0.5 * kSimMass * kSimRadius * kSimRadius;
   }
 
-
   public static final class KickerConstants {
     public static final LoggedTunableNumber kIdleVoltage =
         new LoggedTunableNumber("KickerIdleVoltage", 0.0);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -69,10 +69,10 @@ public RobotContainer() {
 
   /** Configure the subsystems. */
   private void configureSubsystems() {
-    m_aprilTagVision = new AprilTagVision(
-      new AprilTagVisionIONorthstar("northstar_0", null),
-      new AprilTagVisionIONorthstar("northstar_1", null)
-    );
+    m_aprilTagVision =
+        new AprilTagVision(
+            new AprilTagVisionIONorthstar("northstar_0", null),
+            new AprilTagVisionIONorthstar("northstar_1", null));
 
     if (RobotBase.isReal()) {
       m_drive =

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -24,7 +24,8 @@ private RobotState(Drive drive, Intake intake, Kicker kicker, AprilTagVision apr
     m_aprilTagVision = aprilTagVision;
   }
 
-  public static void start(Drive drive, Intake intake, Kicker kicker, AprilTagVision aprilTagVision) {
+  public static void start(
+      Drive drive, Intake intake, Kicker kicker, AprilTagVision aprilTagVision) {
     m_instance = new RobotState(drive, intake, kicker, aprilTagVision);
   }
 

diff --git a/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java
@@ -1,14 +1,5 @@
 package frc.robot.subsystems.aprilTagVision;
 
-import java.util.ArrayList;
-import java.util.Comparator;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-import java.util.Optional;
-
-import org.littletonrobotics.junction.Logger;
-
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
@@ -27,6 +18,13 @@
 import frc.robot.RobotState;
 import frc.robot.subsystems.aprilTagVision.AprilTagVisionIO.AprilTagVisionInputs;
 import frc.robot.util.GeomUtil;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
 
 public class AprilTagVision extends SubsystemBase {
   public record VisionObservation(

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -49,8 +49,8 @@ public void updateState(IntakeState state) {
   }
 
   // Written by Elijah Chen
-  // Nasik Hobbs 
+  // Nasik Hobbs
   // William Lin
   // Completed intajes and kicker today vs
-  //shut up stupid faces you guys stink haha
+  // shut up stupid faces you guys stink haha
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java
@@ -3,17 +3,17 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface FlywheelIO {
-    @AutoLog
-    public class FlywheelInputsAutoLogged {
-        public double topVelocityRadPerSec;
-        public double topVoltage;
-        public double topCurrentAmps;
-        public double bottomVelocityRadPerSec;
-        public double bottomVoltage;
-        public double bottomCurrentAmps;
-    }
-    
-    public void setVoltage(double topVoltage, double bottomVoltage);
-    public void updateInputs(FlywheelInputsAutoLogged inputs);
-    public void setVelocity(double desiredTopVelocity, double desiredBottomVelocity);
+  @AutoLog
+  public class FlywheelInputs {
+    public double topVelocityRPM;
+    public double topVoltage;
+    public double topCurrentAmps;
+    public double bottomVelocityRPM;
+    public double bottomVoltage;
+    public double bottomCurrentAmps;
+  }
+
+  public void setVoltage(double topVoltage, double bottomVoltage);
+
+  public void updateInputs(FlywheelInputs inputs);
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIONeo.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIONeo.java
@@ -4,39 +4,32 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 
-public class FlywheelIONeo implements FlywheelIO{
-    private CANSparkMax m_topMotor;
-    private CANSparkMax m_bottomMotor;
-    private RelativeEncoder m_topEncoder;
-    private RelativeEncoder m_bottomEncoder;
+public class FlywheelIONeo implements FlywheelIO {
+  private CANSparkMax m_topMotor;
+  private CANSparkMax m_bottomMotor;
+  private RelativeEncoder m_topEncoder;
+  private RelativeEncoder m_bottomEncoder;
 
-    public FlywheelIONeo(int topPort, int bottomPort){
-        m_topMotor = new CANSparkMax(topPort, MotorType.kBrushless);
-        m_topEncoder = m_topMotor.getEncoder();
-        m_bottomMotor = new CANSparkMax(bottomPort, MotorType.kBrushless);
-        m_bottomEncoder = m_bottomMotor.getEncoder();
-    }
+  public FlywheelIONeo(int topPort, int bottomPort) {
+    m_topMotor = new CANSparkMax(topPort, MotorType.kBrushless);
+    m_topEncoder = m_topMotor.getEncoder();
+    m_bottomMotor = new CANSparkMax(bottomPort, MotorType.kBrushless);
+    m_bottomEncoder = m_bottomMotor.getEncoder();
+  }
 
-    @Override
-    public void setVoltage(double topVoltage, double bottomVoltage) {
-        m_topMotor.setVoltage(topVoltage);
-        m_bottomMotor.setVoltage(bottomVoltage);
-    }
+  @Override
+  public void setVoltage(double topVoltage, double bottomVoltage) {
+    m_topMotor.setVoltage(topVoltage);
+    m_bottomMotor.setVoltage(bottomVoltage);
+  }
 
-    @Override
-    public void updateInputs(FlywheelInputsAutoLogged inputs) {
-        inputs.topVelocityRadPerSec = m_topEncoder.getVelocity();
-        inputs.topVoltage = m_topMotor.getBusVoltage();
-        inputs.topCurrentAmps = m_topMotor.getOutputCurrent();
-        inputs.bottomVelocityRadPerSec = m_bottomEncoder.getVelocity();
-        inputs.bottomVoltage = m_bottomMotor.getBusVoltage();
-        inputs.bottomCurrentAmps = m_bottomMotor.getOutputCurrent();
-    }
-
-    @Override
-    public void setVelocity(double desiredTopVelocity, double desiredBottomVelocity) {
-        m_topMotor.set(desiredTopVelocity);
-        m_bottomMotor.set(desiredBottomVelocity);
-    }
-
-}
+  @Override
+  public void updateInputs(FlywheelInputs inputs) {
+    inputs.topVelocityRPM = m_topEncoder.getVelocity();
+    inputs.topVoltage = m_topMotor.getBusVoltage() * m_topMotor.getAppliedOutput();
+    inputs.topCurrentAmps = m_topMotor.getOutputCurrent();
+    inputs.bottomVelocityRPM = m_bottomEncoder.getVelocity();
+    inputs.bottomVoltage = m_bottomMotor.getBusVoltage() * m_bottomMotor.getAppliedOutput();
+    inputs.bottomCurrentAmps = m_bottomMotor.getOutputCurrent();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java
@@ -5,34 +5,28 @@
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 
 public class FlywheelIOSim implements FlywheelIO {
-    public FlywheelSim m_topFlywheel;
-    public FlywheelSim m_bottomFlywheel;
-    public final double m_wheelLength; //हज़फ़लदग्लाफ़ंक्ल 
+  public FlywheelSim m_topFlywheel;
+  public FlywheelSim m_bottomFlywheel;
+  public final double m_wheelLength; // हज़फ़लदग्लाफ़ंक्ल
 
-    public FlywheelIOSim() {
-        DCMotor gearbox = DCMotor.getFalcon500(1);
-        double gearing = 1/1.5;
-        double jKgMetersSquared = 0.1;
-        m_topFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared);
-        m_bottomFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared);
-        m_wheelLength = Units.inchesToMeters(694202496);
-      }
+  public FlywheelIOSim() {
+    DCMotor gearbox = DCMotor.getFalcon500(1);
+    double gearing = 1 / 1.5;
+    double jKgMetersSquared = 0.1;
+    m_topFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared);
+    m_bottomFlywheel = new FlywheelSim(gearbox, gearing, jKgMetersSquared);
+    m_wheelLength = Units.inchesToMeters(694202496);
+  }
 
-    @Override
-    public void setVoltage(double topVoltage, double bottomVoltage) {
-        m_topFlywheel.setInputVoltage(topVoltage);
-        m_bottomFlywheel.setInputVoltage(bottomVoltage);
-    }
+  @Override
+  public void setVoltage(double topVoltage, double bottomVoltage) {
+    m_topFlywheel.setInputVoltage(topVoltage);
+    m_bottomFlywheel.setInputVoltage(bottomVoltage);
+  }
 
-    @Override
-    public void updateInputs(FlywheelInputsAutoLogged inputs) {
-        m_topFlywheel.update(.02);
-        m_bottomFlywheel.update(.02);
-    }
-
-    @Override
-    public void setVelocity(double desiredTopVelocity, double desiredBottomVelocity) {
-
-    }
-
-}
+  @Override
+  public void updateInputs(FlywheelInputs inputs) {
+    m_topFlywheel.update(.02);
+    m_bottomFlywheel.update(.02);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -1,51 +1,51 @@
 package frc.robot.subsystems.shooter;
 
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ShooterConstants;
-import frc.robot.subsystems.shooter.FlywheelIO.FlywheelInputsAutoLogged;
+import frc.robot.subsystems.shooter.FlywheelIO.FlywheelInputs;
 import frc.robot.util.SubsystemProfiles;
-
 import java.util.HashMap;
-
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.inputs.LoggableInputs;
 
-public class Shooter extends SubsystemBase{
-    private FlywheelIO m_io;
-    private FlywheelInputsAutoLogged m_inputs;
-    private SubsystemProfiles m_profiles;
-    private ProfiledPIDController m_topController;
-    private ProfiledPIDController m_bottomController;
+public class Shooter extends SubsystemBase {
+  private FlywheelIO m_io;
+  private FlywheelInputs m_inputs;
+  private SubsystemProfiles m_profiles;
+  private PIDController m_topController;
+  private PIDController m_bottomController;
 
-    public enum ShooterState {
-        kIdle,
-        kRevving
-    }
+  public enum ShooterState {
+    kIdle,
+    kRevving
+  }
 
-    public Shooter(FlywheelIO io, ProfiledPIDController topController, ProfiledPIDController bottomController) {
-        m_io = io;
-        m_profiles = new SubsystemProfiles(ShooterState.class, new HashMap<>(), ShooterState.kIdle);
-        m_topController = topController; 
-        m_bottomController = bottomController;
+  public Shooter(FlywheelIO io, PIDController topController, PIDController bottomController) {
+    m_io = io;
+    m_profiles = new SubsystemProfiles(ShooterState.class, new HashMap<>(), ShooterState.kIdle);
+    m_topController = topController;
+    m_bottomController = bottomController;
+  }
+
+  @Override
+  public void periodic() {
+    m_io.updateInputs(m_inputs);
+    Logger.processInputs("Flywheel", (LoggableInputs) m_inputs);
+    Logger.recordOutput("Flywheel/State", (ShooterState) m_profiles.getCurrentProfile());
+    if (m_profiles.currentProfile == ShooterState.kIdle) {
+      // probably wrong PID
+      m_topController.setPID(
+          ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get());
+      m_bottomController.setPID(
+          ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get());
+      m_io.setVoltage(
+          m_topController.calculate(m_inputs.topVelocityRPM),
+          m_bottomController.calculate(m_inputs.bottomVelocityRPM));
     }
+  }
 
-    @Override
-    public void periodic(){
-        m_io.updateInputs(m_inputs);
-        Logger.processInputs("Flywheel", (LoggableInputs) m_inputs);
-        Logger.recordOutput("Flywheel/State", (ShooterState) m_profiles.getCurrentProfile());
-        if(m_profiles.currentProfile == ShooterState.kIdle){
-            // probably wrong PID
-            m_topController.setPID(ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get());
-            m_bottomController.setPID(ShooterConstants.kP.get(), ShooterConstants.kI.get(), ShooterConstants.kD.get());
-            m_topController.setGoal(ShooterConstants.kTopShootingVoltage.get());
-            m_bottomController.setGoal(ShooterConstants.kBottomShootingVoltage.get());
-            m_io.setVoltage(m_topController.getGoal().velocity, m_bottomController.getGoal().velocity);
-        }
-    }   
-
-    public void updateState(ShooterState state) {
+  public void updateState(ShooterState state) {
     switch (state) {
       case kIdle:
         m_profiles.currentProfile = ShooterState.kIdle;
@@ -56,5 +56,10 @@ public void updateState(ShooterState state) {
     }
 
     m_profiles.setCurrentProfile(state);
-}
+  }
+
+  public void setDesiredVelocity(double topVelocity, double bottomVelocity) {
+    m_topController.setSetpoint(topVelocity);
+    m_bottomController.setSetpoint(bottomVelocity);
+  }
 }
diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java
@@ -118,4 +118,4 @@ public static void ifChanged(
   public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
     ifChanged(id, values -> action.run(), tunableNumbers);
   }
-}
+}
-Original file line number
+Diff line change
@@ Expand Up / @@ -118,4 +118,4 @@ public static void ifChanged( @@
       public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
         ifChanged(id, values -> action.run(), tunableNumbers);
       }
-    }
+    }