4 4 spindexer testing

src/main/java/frc/robot/Robot.java

@@ -152,7 +152,7 @@ public enum RobotEdition {
    * This is for when we're testing shot and extension numbers and should be FALSE once bring up is
    * complete
    */
-  public static final boolean TUNING_MODE = false;
+  public static final boolean TUNING_MODE = true;
 
   public boolean hasZeroedSinceStartup = false;
 

src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.components.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOInputsAutoLogged;
+import frc.robot.utils.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -48,6 +49,10 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer {
 
   public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO
 
+  private LoggedTunableNumber kickerSpeed = new LoggedTunableNumber("Kicker Speed", 100);
+
+  private LoggedTunableNumber spinnerSpeed = new LoggedTunableNumber("Spinner Speed", 13);
+
   public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) {
     this.kickerIO = kickerIO;
     this.spinnerIO = indexRollerIO;
@@ -73,8 +78,8 @@ public Command kick() {
               //     .withTimeout(3),
               // this.run(
               //     () -> {
-              spinnerIO.setRollerVelocity(30);
-              kickerIO.setRollerVelocity(20);
+              spinnerIO.setRollerVelocity(spinnerSpeed.get());
+              kickerIO.setRollerVelocity(kickerSpeed.get());
             }));
   }
 
@@ -92,7 +97,7 @@ public Command rest() {
     return this.run(
         () -> {
           spinnerIO.setRollerVoltage(0.0);
-          kickerIO.setRollerVoltage(-2);
+          kickerIO.setRollerVoltage(0.0);
         });
   }
 
@@ -155,7 +160,7 @@ public static TalonFXConfiguration getKickerConfig() {
     config.Slot0.kS = 0.22251;
     config.Slot0.kV = 0.17199;
     config.Slot0.kA = 0.024802;
-    config.Slot0.kP = 7;
+    config.Slot0.kP = 21;
     config.Slot0.kD = 0;
 
     config.CurrentLimits.StatorCurrentLimit = 80.0;

src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java

@@ -11,6 +11,7 @@
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
@@ -66,6 +67,8 @@ public static class FlywheelIOInputs {
   private MotionMagicVelocityVoltage motionMagicVelocityVoltage;
   private VelocityTorqueCurrentFOC velocityTorqueCurrentFOC =
       new VelocityTorqueCurrentFOC(0.0).withSlot(1);
+  private VelocityVoltage velocityVoltage =
+      new VelocityVoltage(0.0).withEnableFOC(true).withSlot(0);
 
   private double velocitySetpointRotPerSec = 100.0; // TODO can't start w 0
 
@@ -141,6 +144,11 @@ public void setTorqueCurrentVel(double flywheelVel) {
     flywheelLeader.setControl(velocityTorqueCurrentFOC.withVelocity(flywheelVel));
   }
 
+  public void setFlywheelVelocity(double flywheelVelocity) {
+    velocitySetpointRotPerSec = flywheelVelocity;
+    flywheelLeader.setControl(velocityVoltage.withVelocity(flywheelVelocity));
+  }
+
   public void stop() { // thought i should add a stop command, dont think i had to though
     velocitySetpointRotPerSec = 0.0;
     flywheelLeader.setControl(voltageOut.withOutput(0.0));

src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java

@@ -252,7 +252,8 @@ public Command feed(
                   Logger.recordOutput("Robot/Feed Target", feedTarget.get());
                   hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle());
                   // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec());
-                  flywheelIO.setMotionProfiledFlywheelVelocity(
+                  // flywheelIO.setMotionProfiledFlywheelVelocity(
+                  flywheelIO.setFlywheelVelocity(
                       shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()
                           + AutoAim.getFudgeFactor());
                   turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle());
@@ -307,7 +308,8 @@ public Command spit() {
     return this.run(
         () -> {
           hoodIO.setHoodPosition(HOOD_MIN_ANGLE);
-          flywheelIO.setMotionProfiledFlywheelVelocity(30);
+          // flywheelIO.setMotionProfiledFlywheelVelocity(
+          flywheelIO.setFlywheelVelocity(30);
           // i think we want it to eject as far out from the robot as possible
           turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5));
         }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY
@@ -352,7 +354,8 @@ public Command score(Supplier<ShotParams> shotParamsSupplier) {
                   //     turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE);
                   //   case NONE:
                   hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle());
-                  flywheelIO.setMotionProfiledFlywheelVelocity(
+                  // flywheelIO.setMotionProfiledFlywheelVelocity(
+                  flywheelIO.setFlywheelVelocity(
                       shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()
                           + AutoAim.getFudgeFactor());
                   turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle());
@@ -505,11 +508,11 @@ public static TalonFXConfiguration getFlywheelConfig() {
 
     config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO;
 
-    // slot 0 is for motion profiled velocity
+    // slot 0 is for velocity
     config.Slot0.kS = 0.33706; // 0.63933;
     config.Slot0.kV = 0.13893; // 0.11582;
     config.Slot0.kA = 0.030026; // 0.020809;
-    config.Slot0.kP = 0.4;
+    config.Slot0.kP = 0.67;
     config.Slot0.kD = 0;
 
     // slot 1 is for torque current