turret calibration

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index d4992d30f0eb9e87e6a83d9506dff4260a937e67..d4cb2a9ba22a17f2f6a32783b47ffdaa9541fd89 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -30,10 +30,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit;
         limitConfig.SupplyCurrentLowerTime = 1.5;
         motor.getConfigurator().apply(limitConfig);
-
-        SmartDashboard.putData("Max speed spindexer", new InstantCommand(() -> maxSpindexer()));
-        SmartDashboard.putData("Turn off spindexer", new InstantCommand(() -> stopSpindexer()));
-        SmartDashboard.putData("Spindexer 50%", new InstantCommand(() -> setSpindexer(0.5)));
     }
 
     @Override
@@ -45,7 +41,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         
         // scale threshold based on power
         double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power;
-        SmartDashboard.putNumber("Spindexer Velocity Threshold", velocityThreshold);
         SmartDashboard.putNumber("Spindexer Ball Count", ballCount);
 
         boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold;

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8ac2b12ad6e86250da6ea8bebe1eb860d2bb3d54..363927beccc0415fa66917d1bdabb548f800041a 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -2,6 +2,7 @@ package frc.robot.subsystems.turret;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
@@ -20,6 +21,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
@@ -31,6 +33,8 @@ public class Turret extends SubsystemBase implements TurretIO{
 
     private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
 
+       private boolean calibrating;
+
        /* ---------------- Hardware ---------------- */
 
        private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB);
@@ -75,6 +79,8 @@ public class Turret extends SubsystemBase implements TurretIO{
                mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
         motor.getConfigurator().apply(config);
 
+               setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
+
                lastGoalRad = 0.0;
 
                if (RobotBase.isSimulation()) {
@@ -203,10 +209,17 @@ public class Turret extends SubsystemBase implements TurretIO{
                // Multiply goal velocity by kV
                double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV;
 
-               // Sets motor control with feedforward
-               motor.setControl(mmVoltageRequest
+               if(calibrating){
+                       motor.set(0.05);
+                       if(Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD){
+                               stopCalibrating();
+                       }
+               } else{
+                       // Sets motor control with feedforward
+                       motor.setControl(mmVoltageRequest
                        .withPosition(motorGoalRotations)
                        .withFeedForward(robotTurnCompensation));
+               }
 
         Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
                Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees());
@@ -221,7 +234,8 @@ public class Turret extends SubsystemBase implements TurretIO{
                SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble());
                SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble());
 
-
+               SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate()));
+               SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating()));
        }
 
        /* ---------------- Simulation ---------------- */
@@ -248,10 +262,41 @@ public class Turret extends SubsystemBase implements TurretIO{
                inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
        }
 
+       /**
+     * sets supply and stator current limits
+     * @param limit the current limit for stator and supply current
+     */
+    public void setCurrentLimits(double limit) {
+        CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
+        .withStatorCurrentLimitEnable(true)
+        .withStatorCurrentLimit(limit)
+        .withSupplyCurrentLimitEnable(true)
+        .withSupplyCurrentLimit(limit);
+
+               if(limit == TurretConstants.NORMAL_CURRENT_LIMIT){
+                       limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT;
+                       limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second
+               }
+
+        motor.getConfigurator().apply(limits);
+    }
+
        // Also ignore this for now
        private double wrapUnit(double value) {
                value %= 1.0;
                if (value < 0) value += 1.0;
                return value;
        }
+
+       private void calibrate(){
+               setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT);
+               calibrating = true;
+       }
+
+       private void stopCalibrating(){
+               motor.set(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO);
+               setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
+               calibrating = false;
+               setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0);
+       }
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 44425c0dc31a5c53143c2528a832e3bffbebf216..83dd1c3953237a46a2e744831d2d55a6478e0364 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -7,6 +7,8 @@ public class TurretConstants {
     public static double MAX_ANGLE = 200; // Deg
     public static double MIN_ANGLE = -200; // Deg
 
+    public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop
+
     public static double MAX_VELOCITY = 600; // rad/s
     public static double MAX_ACCELERATION = 120.0; // rad/s^2
 
@@ -31,4 +33,9 @@ public class TurretConstants {
        public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06;
 
        public static final double FEEDFORWARD_KV = 0.185;
+
+    public static final double NORMAL_CURRENT_LIMIT = 50.0; // A
+    public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A
+    public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
+
 }