ofrofja

diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelPhase.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelPhase.java
deleted file mode 100644 (file)
index 7b43ecf..0000000
--- a/src/main/java/frc/robot/subsystems/shooter/FlywheelPhase.java
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc.robot.subsystems.shooter;
-
-public enum FlywheelPhase {
-    MAX,
-    CONSTANT_TORQUE,
-    START_UP
-}

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 34ccfb01063f335e06a59e8837fd1093b42305eb..05156fcd0b195b304ae2a27642d58de47cae2b42 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -51,7 +51,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     private boolean lastTorqueCurrentControl = false;
 
     public enum FlywheelPhase {
-        MAX,
+        BANG_BANG,
         CONSTANT_TORQUE,
         START_UP
     }    
@@ -90,11 +90,11 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     public void periodic(){
         updateInputs();
-        //updatePhase();
+        runVelocity(Units.rotationsToRadians(getShooterVelcoity()));
         SmartDashboard.putNumber("Shot Power", shooterPower);
         shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
 
-        if (phase == FlywheelPhase.MAX) { // shooter target speed is in RPS
+        if (phase == FlywheelPhase.BANG_BANG) { // shooter target speed is in RPS
             // Duty-cycle bang-bang (apply to both)
             shooterMotorLeft.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // spin as fast as possible shooter target speed
             shooterMotorRight.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // same here
@@ -108,14 +108,6 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         // shooterMotorRight.set(-shooterPower);
     }
 
-    public void updatePhase() {
-        if (Math.abs(shooterMotorLeft.getVelocity().getValueAsDouble() - shooterTargetSpeed) < ShooterConstants.TORQUE_CURRENT_CONTROL_TOLERANCE) {
-            phase = FlywheelPhase.MAX; // we need to recover from ball velocity in order to be back to idle
-        } else {
-            phase = FlywheelPhase.CONSTANT_TORQUE;                                                             
-        }
-    }
-
     /** Run closed loop at the specified velocity. */
     private void runVelocity(double velocityRadsPerSec) {
         boolean inTolerance =
@@ -129,11 +121,11 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         }
         lastTorqueCurrentControl = torqueCurrentControl;
 
-        outputs.mode =
+        phase =
             torqueCurrentControl
-                ? FlywheelIOOutputMode.TORQUE_CURRENT_BANG_BANG
-                : FlywheelIOOutputMode.DUTY_CYCLE_BANG_BANG;
-        outputs.velocityRadsPerSec = velocityRadsPerSec;
+                ? FlywheelPhase.BANG_BANG
+                : FlywheelPhase.CONSTANT_TORQUE;
+        shooterTargetSpeed = Units.radiansToRotations(velocityRadsPerSec);
         Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec);
     }