ok I added second motor and voltage control

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 15c3fecc54d2ccdb9b2c726a3288fb4b4d3f7bfc..9530c11d3f232b45779cd696967dba50fc3bc509 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -251,8 +251,6 @@ public class RobotContainer {
       NamedCommands.registerCommand("Start Spindexer",
           new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer)));
       NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel()));
-      NamedCommands.registerCommand("Reset Spindexer", new InstantCommand(() -> spindexer.resetSpindexer()));
-      NamedCommands.registerCommand("Reset Reset Angle", new InstantCommand(() -> spindexer.resetResetAngle()));
     }
 
     if (hood != null) {

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index 160c8da00e32e990f0fe3455c59454b620ad24f3..a662da5c23827f0898f26699637366e619ac60e6 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -32,10 +32,8 @@ public class IdConstants {
     public static final int HOOD_ID = 11;
 
     // Spindexer
-    public static final int SPINDEXER_ID = 4;
-    
-    // Climb
-    public static final int CLIMB_MOTOR_ID = 8;
+    public static final int SPINDEXER_ONE_ID = 4;
+    public static final int SPINDEXER_TWO_ID = 8;
 
     // Intake
     public static final int RIGHT_MOTOR_ID = 1;

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 80389afa2d0e0c700188f7efecae35dce6f790a8..5eccdc161aa57894aec5a6e9a7b61031b7fa67d7 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -45,7 +45,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
             ClimbConstants.PID_D);
 
     public LinearClimb() {
-        motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.CANIVORE_SUB);
+        motor = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB);
         pid.setTolerance(ClimbConstants.PID_TOLERANCE);
 
         motor.setNeutralMode(NeutralModeValue.Brake);

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index e9cb913d10c9ece92e084a7c97040ffd93f90231..d9822c59bed9413c26369b1464d84ff18ed6a1f3 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -2,6 +2,7 @@ package frc.robot.subsystems.spindexer;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import org.littletonrobotics.junction.Logger;
@@ -14,7 +15,8 @@ import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
 
 public class Spindexer extends SubsystemBase implements SpindexerIO {
-    private TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID, Constants.CANIVORE_SUB);
+    private TalonFX motorOne = new TalonFX(IdConstants.SPINDEXER_ONE_ID, Constants.CANIVORE_SUB);
+    private TalonFX motorTwo = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB);
 
     private double power = 0.0;
     public int ballCount = 0;
@@ -35,22 +37,20 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         limitConfig.StatorCurrentLimitEnable = true;
         limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit;
         limitConfig.SupplyCurrentLowerTime = 1.5;
-        motor.getConfigurator().apply(limitConfig);
+        motorOne.getConfigurator().apply(limitConfig);
+        motorTwo.getConfigurator().apply(limitConfig);
 
         if (!Constants.DISABLE_SMART_DASHBOARD) {
             SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer()));
             SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer()));
             SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer()));
         }
-
-        resetPID.setTolerance(0.05);
     }
 
     public enum SpindexerState {
         MAX,
         REVERSE,
         STOPPED,
-        RESET,
         CUSTOM,
     }
 
@@ -59,49 +59,36 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         updateInputs();
         Logger.processInputs("Spindexer", inputs);
 
-        if (resetPos == null) {
-            motor.setPosition(0.1 * gearRatio);
-            resetPos = (motor.getPosition().getValueAsDouble() / gearRatio) % 1.0;
-            resetPID.reset();
-        }
-
         if (state == SpindexerState.MAX) {
-            motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerMaxPower).withEnableFOC(true));
+            setMotorVoltages(SpindexerConstants.spindexerForwardVoltage);
             reversing = false;
         } else if (state == SpindexerState.REVERSE) {
-            motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerReversePower).withEnableFOC(true));
+            setMotorVoltages(SpindexerConstants.spindexerReverseVoltage);
             reversing = true;
         } else if (state == SpindexerState.STOPPED) {
-            motor.setControl(new DutyCycleOut(0.0).withEnableFOC(true));
+            setMotorVoltages(0.0);
             reversing = false;
-        } else if (state == SpindexerState.RESET && resetPos != null) {
-            motor.setControl(new DutyCycleOut(resetPID.calculate((motor.getPosition().getValueAsDouble() / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
         } else {
-            motor.setControl(new DutyCycleOut(power).withEnableFOC(true));
+            setMotorVoltages(power);
             reversing = false;
         }
 
 
-        // scale threshold based on power
-        double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power;
         if (!Constants.DISABLE_SMART_DASHBOARD) {
-            SmartDashboard.putNumber("Spindexer Ball Count", ballCount);
-
             SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM);
             SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0);
         }
 
-        boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold;
-        if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) {
-            ballCount++;
-        }
-        wasSpindexerSlow = isSpindexerSlow;
-
         if (!Constants.DISABLE_SMART_DASHBOARD) {
             SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE);
         }
     }
 
+    public void setMotorVoltages(double voltage) {
+        motorOne.setControl(new VoltageOut(voltage).withEnableFOC(true));
+        motorTwo.setControl(new VoltageOut(voltage).withEnableFOC(true));
+    }
+
     public void maxSpindexer() {
         state = SpindexerState.MAX;
     }
@@ -119,16 +106,8 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         state = SpindexerState.CUSTOM;
     }
 
-    public void resetSpindexer() {
-        state = SpindexerState.RESET;
-    }
-
-    public void resetResetAngle() {
-        resetPos = null;
-    }
-
     public double getStatorCurrent() {
-        return inputs.spindexerCurrent;
+        return inputs.spindexerOneCurrent + inputs.spindexerTwoCurrent;
     }
 
     public void setNewCurrentLimit(double newCurrentLimit) {
@@ -137,20 +116,15 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         limitConfig.StatorCurrentLimitEnable = true;
         limitConfig.SupplyCurrentLowerLimit = newCurrentLimit;
         limitConfig.SupplyCurrentLowerTime = 1.5;
-        motor.getConfigurator().apply(limitConfig);
+        motorOne.getConfigurator().apply(limitConfig);
+        motorTwo.getConfigurator().apply(limitConfig);
     }
 
     @Override
     public void updateInputs() {
-        inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio;
-        inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble();
+        inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble();
+        inputs.spindexerOneCurrent = motorOne.getStatorCurrent().getValueAsDouble();
+        inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble();
+        inputs.spindexerTwoCurrent = motorTwo.getStatorCurrent().getValueAsDouble();
     }
-
-    private Double resetPos;
-    private PIDController resetPID = new PIDController(4.0, 0.0, 0);
-
-    private final double gearRatio = 27.0 / 1.0; //spindexer spins once for every 27 motor spins
-
-    
-
 }

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
index a644b98cf94e86ed47e68151192ac5256a86980d..1855440a41635e441c10f27ceba01e0a0a79b6a4 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
@@ -3,8 +3,10 @@ package frc.robot.subsystems.spindexer;
 public class SpindexerConstants {
     public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
     public static final double currentLimit = 40; // A
-    public static final double spindexerMaxPower = 1.00; 
-    public static final double spindexerReversePower = -1.00;
+    public static final double spindexerForwardVoltage = 1.00; // Volts (set low for testing)
+    public static final double spindexerReverseVoltage = -1.00; // Volts
+    public static final double GEAR_RATIO = 27.0; // unused & both motors have same gearing
+
     public static final double CURRENT_SPIKE_LIMIT = 150;
     public static final double CURRENT_TIME_LIMIT = 1.0; //s
     public static final double JAM_CURRENT_THRESHOLD = 75.0; // A

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
index 1cf6740df9d18c562de35a89619a6bd0f706e3a2..e1a154f6fe6f38d2e47a0e744b20393a58335842 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
@@ -5,8 +5,10 @@ import org.littletonrobotics.junction.AutoLog;
 public interface SpindexerIO {
     @AutoLog
     public static class SpindexerIOInputs {
-        public double spindexerVelocity = 0.0;
-        public double spindexerCurrent = 0.0;
+        public double spindexerOneVelocity = 0.0;
+        public double spindexerOneCurrent = 0.0;
+        public double spindexerTwoVelocity = 0.0;
+        public double spindexerTwoCurrent = 0.0;
     }
 
     public void updateInputs();