i'll finish ts later

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index a8581f1d0aaa5c74584710311714a9921260c431..9c689e56668984dbbf06174e032e806781898b96 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -35,6 +35,27 @@ public class AutoShootCommand extends Command {
     private Shooter shooter;
     private Spindexer spindexer;
 
+    private enum WantedState {
+        IDLE,
+        SHOOTING,
+        PASSING
+    }
+
+    private enum CurrentState {
+        IDLE,
+        STARTING_UP,
+        TURNING_AROUND,
+        SHOOTING,
+        PASSING
+    }
+
+    private WantedState wantedState = WantedState.IDLE;
+    private CurrentState currentState = CurrentState.IDLE;
+
+    private void updateStates(){
+
+    }
+
     //TODO: find maximum interpolation
     private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
 
@@ -162,7 +183,7 @@ public class AutoShootCommand extends Command {
         updateSetpoints(drivepose);
         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
         hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
-        shooter.setShooter(Units.radiansToRotations(goalState.exitVel() / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)));
+        shooter.setShooter(goalState.exitVel());
 
         /** Spindexer Stuff!! */
         if(spindexer != null){

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 0299e4df508d348da1521b987f3d626d7265df00..6eb011e6c4e393c9c0ce4410cdc42f4d87223d37 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -108,9 +108,13 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         return inputs.leftShooterVelocity; 
     }
 
+    public boolean atTargetVelocity(){
+        return Math.abs(getShooterVelcoity() - targetSpeedMPS) < 0.1; // Tolerance of 0.1 mps
+    }
+
     public void updateInputs() {
-        inputs.leftShooterVelocity = motorLeft.getVelocity().getValueAsDouble();
-        inputs.rightShooterVelocity = motorRight.getVelocity().getValueAsDouble();
+        inputs.leftShooterVelocity = Units.rotationsToRadians(motorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
+        inputs.rightShooterVelocity =Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
         inputs.leftShooterCurrent = motorLeft.getStatorCurrent().getValueAsDouble();
         inputs.rightShooterCurrent = motorRight.getStatorCurrent().getValueAsDouble();
     }