Convert shooting method

Author: Nathanael7639

src/main/java/frc/robot/Constants.java

@@ -45,6 +45,7 @@ public class FLYWHEEL {
     public static final double peakReverseOutput = -0.35; // Placeholder value
     public static final double kInertia = 0.01;
     public static final double kStatorCurrentLimit = 120;
+    public static final double kVelocityErrorThreshold = 35;
 
     // These worked on wood bot. Change jerk later if further optimization is needed
     public static double motionMagicCruiseVelocity = 60.0; // target cruise velocity of 60 rps
@@ -414,6 +415,7 @@ public class UPTAKE {
     public static final double peakForwardOutput = 0.5;
     public static final double peakReverseOutput = -0.5;
     public static final double kInertia = 0.01;
+    public static final double kVelocityErrorThreshold = 50.0;
 
     public static final double kMotionMagicAcceleration = 30.0;
     public static final double kMotionMagicCruiseVelocity = 60.0;

src/main/java/frc/robot/commands/shooter/Shoot.java

@@ -1,6 +1,7 @@
 package frc.robot.commands.shooter;
 
 import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.math.geometry.Translation2d;
@@ -60,7 +61,7 @@ public void initialize() {
   @Override
   public void execute() {
     Shot shot = distanceToShotMap.get(m_vision.getDistanceToHub());
-    m_flywheel.setRPMOutputFOC(shot.shooterRPM);
+    m_flywheel.setRPMOutputFOC(RPM.of(shot.shooterRPM));
     // m_shooterHood.setPosition(shot.hoodPosition);
   }
 

src/main/java/frc/robot/commands/shooter/ShootManualFlywheel.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.shooter;
 
+import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.wpilibj2.command.Command;
@@ -22,7 +23,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_flywheel.setRPMOutputFOC(m_flywheel.getRPMSetpoint());
+    m_flywheel.setRPMOutputFOC(RPM.of(m_flywheel.getRPMSetpoint()));
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/Flywheel.java

@@ -7,6 +7,7 @@
 import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC;
 import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -20,6 +21,7 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DutyCycle;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
@@ -48,7 +50,8 @@ public class Flywheel extends SubsystemBase {
 
   private final MotionMagicVelocityTorqueCurrentFOC m_request =
       new MotionMagicVelocityTorqueCurrentFOC(0);
-  private final TorqueCurrentFOC m_TorqueCurrentFOC = new TorqueCurrentFOC(0);
+  private final DutyCycleOut m_DutyCycleOut = new DutyCycleOut(0);
+  private final TorqueCurrentFOC m_TorqueCurrentFOC = new TorqueCurrentFOC(0.0);
   private static AngularVelocity m_rpmSetpoint = MANUAL_RPM.IDLE.getRPM();
 
   public final DoubleSubscriber m_rpmSubscriber;
@@ -79,7 +82,7 @@ public Flywheel() {
     config.MotorOutput.NeutralMode = m_neutralMode;
     config.Feedback.SensorToMechanismRatio = FLYWHEEL.gearRatio;
     config.CurrentLimits.StatorCurrentLimit = FLYWHEEL.kStatorCurrentLimit;
-    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.StatorCurrentLimitEnable = false;
 
     config.MotionMagic.MotionMagicCruiseVelocity = FLYWHEEL.motionMagicCruiseVelocity;
     config.MotionMagic.MotionMagicAcceleration = FLYWHEEL.motionMagicAcceleration;
@@ -107,9 +110,8 @@ public void changeNeutralMode(NeutralModeValue neutralmode) {
     m_neutralMode = neutralmode;
   }
 
-  public void setRPMOutputFOC(double rpm) {
-    m_rpmSetpoint = RPM.of(rpm);
-    m_motor1.setControl(m_request.withVelocity(m_rpmSetpoint.abs(RotationsPerSecond)));
+  public void setRPMOutputFOC(AngularVelocity rpm) {
+    m_rpmSetpoint = rpm;
   }
 
   public void setTorqueCurrentOutputFOC(Voltage voltage) {
@@ -168,8 +170,18 @@ public void testPeriodic() {
     m_rpmSetpoint = RPM.of(m_rpmSubscriber.get());
   }
 
+  public double getRPMerror(){
+    return getRPMSetpoint() - getMotorSpeedRPM();
+  }
+
   @Override
-  public void periodic() {}
+  public void periodic() {
+    if (Math.abs(getRPMerror()) > FLYWHEEL.kVelocityErrorThreshold){
+      m_motor1.setControl(m_DutyCycleOut.withOutput(Math.signum(getRPMerror())));
+    } else {
+      m_motor1.setControl(m_request.withVelocity(m_rpmSetpoint.abs(RPM)));
+    }
+  }
 
   @Override
   public void simulationPeriodic() {

src/main/java/frc/robot/subsystems/Uptake.java

@@ -9,6 +9,7 @@
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -43,6 +44,7 @@ public class Uptake extends SubsystemBase {
 
   private MotionMagicVelocityTorqueCurrentFOC m_request =
       new MotionMagicVelocityTorqueCurrentFOC(0.0);
+  private DutyCycleOut m_DutyCycleOut = new DutyCycleOut(0.0);
 
   private static AngularVelocity m_velocitySetpoint = RPM.of(0.0);
 
@@ -97,6 +99,10 @@ public double getPercentOutput() {
     return m_motor.get();
   }
 
+  public double getRPMerror(){
+    return getRPMsetpoint() - getMotorSpeedRPM();
+  }
+
   @Logged(name = "Motor Velocity RPM", importance = Logged.Importance.INFO)
   public double getMotorSpeedRPM() {
     return m_motor.getVelocity().refresh().getValue().in(RPM);
@@ -112,7 +118,11 @@ public void testPeriodic() {
 
   @Override
   public void periodic() {
-    m_motor.setControl(m_request.withVelocity(m_velocitySetpoint.abs(RotationsPerSecond)));
+    if (Math.abs(getRPMerror()) > UPTAKE.kVelocityErrorThreshold){
+      m_motor.setControl(m_DutyCycleOut.withOutput(Math.signum(getRPMerror())));
+    } else {
+      m_motor.setControl(m_request.withVelocity(m_velocitySetpoint.abs(RotationsPerSecond)));
+    }
   }
 
   @Override