a

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index 0e544c7aa9591f28cb9240db07ee5fa5d20d8a80..b0bb36eb5a6a2dc97b9e1bac91e5289f25187ede 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
@@ -142,7 +142,7 @@ public class SimpleAutoShoot extends Command {
         Logger.recordOutput("Spinny accel", drivetrain.getAngularRate(2));
         Logger.recordOutput("Original Turret Setpoint", turretSetpoint);
         
-        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) - velocityAdjustment);
+        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
         // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3);
         //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity);
 

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8daa7872974602a02c9cdbdb28afecb1ccb8697c..740851d9d07c92d7250a982191700fbe666306d9 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -98,17 +98,19 @@ public class Turret extends SubsystemBase implements TurretIO{
        /* ---------------- Constructor ---------------- */
 
        public Turret() {
-               motor.setNeutralMode(NeutralModeValue.Coast);
-
-               motor.getConfigurator().apply(
-                               new Slot0Configs()
-                                               .withKP(kP)
-                                               .withKD(kD));
+               motor.setNeutralMode(NeutralModeValue.Brake);
 
                TalonFXConfiguration config = new TalonFXConfiguration();
-               var motionMagicConfigs = config.MotionMagic;
-        motionMagicConfigs.MotionMagicCruiseVelocity = 10 * GEAR_RATIO;
-        motionMagicConfigs.MotionMagicAcceleration = 50 * GEAR_RATIO;
+    
+               config.Slot0.kP = 2.0; 
+               config.Slot0.kS = 0.1; // Static friction compensation
+               config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
+               config.Slot0.kD = 0.1; // The "Braking" term to stop overshoot
+
+               var mm = config.MotionMagic;
+               mm.MotionMagicCruiseVelocity = Units.radiansToRotations(MAX_VEL_RAD_PER_SEC) * GEAR_RATIO;
+               mm.MotionMagicAcceleration = Units.radiansToRotations(40.0) * GEAR_RATIO; // Lowered for belt safety
+               mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed -- maybe 10-20x the acceleration
         motor.getConfigurator().apply(config);
 
                motor.getConfigurator().apply(
@@ -180,56 +182,21 @@ public class Turret extends SubsystemBase implements TurretIO{
 
                lastGoalRad = best;
 
-               // --- Profile in MECHANISM SPACE ---
-               State goalState = new State(
-                               MathUtil.clamp(best, MIN_ANGLE_RAD, MAX_ANGLE_RAD),
-                               goalVelocityRadPerSec);
-
-               setpoint = profile.calculate(
-                               Constants.LOOP_TIME,
-                               setpoint,
-                               goalState);
-
-               // --- Convert to MOTOR SPACE ---
-               double motorPosRot = Units.radiansToRotations(setpoint.position) * GEAR_RATIO;
-
-               double motorVelRotPerSec = Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO;
-
-               double targetVelocity;
-
-               // if(Math.abs(setpoint.position * GEAR_RATIO - Units.rotationsToRadians(motor.getPosition().getValueAsDouble())) > Math.PI/2){
-               //      // in rad/sec
-               //      targetVelocity = longVelocityPID.calculate(
-               //                      motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
-               //                      setpoint.position * GEAR_RATIO);
-                       
-               //      targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
-
-               //      double voltage = feedForward.calculate(targetVelocity);
-               //      motor.setVoltage(voltage);
-               // } else{
-                       // in rad/sec
-                       //double robotRotAcceleration = (Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) - lastFrameVelocity) / Constants.LOOP_TIME;
-                       double motorSetpointPosition = (setpoint.position) * GEAR_RATIO;
-
-                       targetVelocity = positionPID.calculate(
-                                       motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
-                                       motorSetpointPosition);
-                       
-                       targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
-
-                       double voltage = feedForward.calculate(targetVelocity);
-
-                       double velocityCorrectionVoltage = velocityPID.calculate(Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()), targetVelocity);
-                       voltage += velocityCorrectionVoltage;
+               // Tells the Kraken to get to this position using 1000Hz profile
+               double motorGoalRotations = Units.radiansToRotations(best) * GEAR_RATIO;
+               
+               // Add the feedforward for the target velocity (SOTM) here as well
+               double feedforwardVoltage = feedForward.calculate(goalVelocityRadPerSec * GEAR_RATIO);
+               
+               motor.setControl(mmVoltageRequest
+                       .withPosition(motorGoalRotations)
+                       .withFeedForward(feedforwardVoltage));
 
-                       motor.setVoltage(voltage);
-               // }
                lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble());
 
                // var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false);
         Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
-               Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO);
+               //Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO);
 
                // --- Position + velocity feedforward (MA-style) ---
                // motor.setControl(request);
@@ -238,21 +205,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                // --- Visualization ---
                ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
 
-        SmartDashboard.putData(positionPID);
-               SmartDashboard.putData(velocityPID);
-               SmartDashboard.putNumber("Turret GoalDeg",
-                               Units.radiansToDegrees(best));
-               SmartDashboard.putNumber("Turret SetpointDeg",
-                               Units.radiansToDegrees(setpoint.position));
-               SmartDashboard.putNumber("Turret Raw Setpoint", Units.radiansToDegrees(best));
-               SmartDashboard.putNumber("Turret motorPosRot",
-                               Units.radiansToDegrees(motorPosRot));
-               SmartDashboard.putNumber("Turret motorVelRotPerSec",
-                               Units.radiansToDegrees(motorVelRotPerSec));
-        SmartDashboard.putNumber("Turret targetVelocity",
-                               Units.radiansToDegrees(targetVelocity));
-               SmartDashboard.putNumber("Turret Position Deg",
-                               Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO);
        }
 
        /* ---------------- Simulation ---------------- */