updates

src/main/java/frc/robot/experimental/SetShooterVelocity.java

@@ -1,7 +1,6 @@
 package frc.robot.experimental;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.ShooterSubsystem2;
 
 public class SetShooterVelocity extends Command {
@@ -23,13 +22,13 @@ public void initialize() {
 
     @Override
     public void execute() {
-        shooter.setShooterVelocity(velocity);
+        shooter.setTargetFlywheelVelocity(velocity);
 
     }
 
     @Override
     public void end(boolean interrupted) {
-        shooter.setShooterVelocity(0);
+        shooter.setTargetFlywheelVelocity(0);
     }
 
     @Override

src/main/java/frc/robot/experimental/ShootWhenReady.java

@@ -1,6 +1,7 @@
 package frc.robot.experimental;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.ShooterSubsystem2;
 
@@ -23,8 +24,9 @@ public void initialize() {
 
     @Override
     public void execute() {
-        if (shooter.isShooterAtTargetVelocity() & index.HasCargo()) {
-            index.RunIndexing();
+        if (shooter.isFlywheelAtTarget() & index.HasCargo()) {
+            // index.RunIndexing();
+            index.SetPower(Constants.IndexConstants.INDEX_POWER);
         } else {
             index.SetPower(0);
         }
@@ -33,7 +35,7 @@ public void execute() {
 
     @Override
     public void end(boolean interrupted) {
-        shooter.setShooterVelocity(0);
+        shooter.setTargetFlywheelVelocity(0);
     }
 
     @Override

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

@@ -10,7 +10,6 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
-import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
@@ -30,7 +29,7 @@ public class ShooterSubsystem2 extends SubsystemBase{
     private VelocityVoltage m_velocity = new VelocityVoltage(0);
     private double flywheelVelocity;
     private boolean flywheelDisabled = false;
-    private double targetFlywheelSpeed;
+    private double targetFlywheelVelocity;
 
 
     public ShooterSubsystem2() {
@@ -41,9 +40,6 @@ public ShooterSubsystem2() {
         m_primaryMotor.getConfigurator().apply(new TalonFXConfiguration());
         m_secondaryMotor.getConfigurator().apply(new TalonFXConfiguration());
 
-        m_primaryMotor.setInverted(false);
-        m_secondaryMotor.setInverted(false);       
-
         /* Various Configs */
         var flywheelConfigs0 = new Slot0Configs();        
             flywheelConfigs0
@@ -53,13 +49,11 @@ public ShooterSubsystem2() {
                 .withKS(0.0001)
                 .withKV(0.12);
 
-
         var flywheelVelocityConfig = new VoltageConfigs();
             flywheelVelocityConfig
-                .withPeakForwardVoltage(12)  // FRC 2910 running 12
-                .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away
+                .withPeakForwardVoltage(8)  // FRC 2910 running 12
+                .withPeakReverseVoltage(8); // Originally -8, with negative the "helper" text goes away
 
-
         var flywheelCurrentConfigs = new CurrentLimitsConfigs();
         flywheelCurrentConfigs
                 .withStatorCurrentLimit(60) 
@@ -81,33 +75,58 @@ public ShooterSubsystem2() {
 
         m_primaryMotor.getConfigurator().apply(flywheelMotorOutput);
         m_secondaryMotor.getConfigurator().apply(flywheelMotorOutput);
-
+
+        m_primaryMotor.setInverted(true);
+        m_secondaryMotor.setInverted(true);       
 
     } // end of constructor
 
+    /* 
+    // Reference: 1
     /* Sets the velocity the motors are using */
-    public void setTargetFlywheelVelocity(double flywheelVelocity) {
-        this.flywheelVelocity = flywheelVelocity;
-        // m_primaryMotor.setControl(m_primaryVelocity.withVelocity(flywheelVelocity));
-//        m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(flywheelVelocity * 0.95));
+    public void setTargetFlywheelVelocity(double targetFlywheelVelocity) {
+        this.targetFlywheelVelocity = targetFlywheelVelocity;
+        /*
+        m_primaryMotor.setControl(m_primaryVelocity.withVelocity(flywheelVelocity));
+        m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(flywheelVelocity * 0.95));
+        */
     }
 
+    // Reference: 2
+    /* 
     public void setTargetFlywheelSpeed(double targetFlywheelSpeed) {
         this.targetFlywheelSpeed = targetFlywheelSpeed;
     }
+    */
 
+    // Reference: 3
     /* Returns the velocity of the flywheel */
+    /*
     public double getTargetFlywheelSpeed(double targetFlywheelSpeed) {
         return targetFlywheelSpeed;
     }
+     */
 
+    // Reference: 4
     // Returns the velocity of the flywheel 
-    public double getFlywheelVelocity() {
-        return m_primaryMotor.getVelocity() ; // TODO check if this is the right method
-    }
 
-    public boolean isFlywheelAtTargetVelocity() {
-        return Math.abs(getFlywheelVelocity() - targetFlywheelVelocity) < 5; // TODO arbitrary error for now 
+    public StatusSignal<Double> getFlywheelVelocity() {
+        return m_primaryMotor.getVelocity();
     }
+
+    StatusSignal<Double> currentFlywheelVelocity = getFlywheelVelocity();
 
+    // currentFlywheelVelocity = getFlywheelVelocity();
+
+    public boolean isFlywheelAtTarget() {
+        double velocityMarginOfError = 5.0; // TODO: Adjust this value as needed
+        if (Math.abs(targetFlywheelVelocity - currentFlywheelVelocity.getValue()) < velocityMarginOfError)
+            return true;
+        else
+            return false;
+        } // end of logic test
+
+    // call this method with the target velocity as an argument
+    // boolean isAtTarget = isFlywheelAtTarget(targetFlywheelSpeed);
+
 } // end of class ShooterSubsystem2