From bcf5e5129a60c3f5e5552307b50a3514f9381866 Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Sat, 31 Jan 2026 11:11:29 -0800 Subject: [PATCH] Use MotionMagicVelocityVoltage instead. --- .../frc/robot/subsystems/turret/Turret.java | 319 +++++++++--------- 1 file changed, 156 insertions(+), 163 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index e417249..878005c 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,220 +1,213 @@ package frc.robot.subsystems.turret; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; 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; public class Turret extends SubsystemBase { - /* ---------------- Constants ---------------- */ + /* ---------------- Constants ---------------- */ + + private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE); + private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE); + + private static final double MAX_VEL_RAD_PER_SEC = 16.0; + private static final double MAX_ACCEL_RAD_PER_SEC2 = 80.0; + + private static final double VERSA_RATIO = 5.0; + private static final double TURRET_RATIO = 140.0 / 10.0; + private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO; + + private static final PIDController velocityPid = new PIDController(1, 0, 0); + + /* ---------------- Hardware ---------------- */ + + private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN); + + private TalonFXSimState simState; + private SingleJointedArmSim turretSim; + + /* ---------------- Control ---------------- */ + + private final TrapezoidProfile profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints( + MAX_VEL_RAD_PER_SEC, + MAX_ACCEL_RAD_PER_SEC2)); + + private State setpoint = new State(); + + private Rotation2d goalAngle = Rotation2d.kZero; + private double goalVelocityRadPerSec = 0.0; + private double lastGoalRad = 0.0; - private static final double MIN_ANGLE_RAD = - Units.degreesToRadians(TurretConstants.MIN_ANGLE); - private static final double MAX_ANGLE_RAD = - Units.degreesToRadians(TurretConstants.MAX_ANGLE); + /* ---------------- Gains ---------------- */ - private static final double MAX_VEL_RAD_PER_SEC = 16.0; - private static final double MAX_ACCEL_RAD_PER_SEC2 = 80.0; + private static final double kP = 3500.0; - private static final double VERSA_RATIO = 5.0; - private static final double TURRET_RATIO = 140.0 / 10.0; - private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO; + private static final double kD = 150.0; - /* ---------------- Hardware ---------------- */ + /* ---------------- Visualization ---------------- */ - private final TalonFX motor = - new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN); + private final Mechanism2d mech = new Mechanism2d(100, 100); + private final MechanismRoot2d root = mech.getRoot("turret", 50, 50); + private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0)); - private TalonFXSimState simState; - private SingleJointedArmSim turretSim; + /* ---------------- Constructor ---------------- */ - /* ---------------- Control ---------------- */ + public Turret() { + motor.setNeutralMode(NeutralModeValue.Coast); - private final TrapezoidProfile profile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - MAX_VEL_RAD_PER_SEC, - MAX_ACCEL_RAD_PER_SEC2)); + motor.getConfigurator().apply( + new Slot0Configs() + .withKP(kP) + .withKD(kD)); - private State setpoint = new State(); + motor.getConfigurator().apply( + new com.ctre.phoenix6.configs.TalonFXConfiguration() { + { + MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } + }); - private Rotation2d goalAngle = Rotation2d.kZero; - private double goalVelocityRadPerSec = 0.0; - private double lastGoalRad = 0.0; + motor.getConfigurator().apply( + new TalonFXConfiguration() { + { + Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; + } + }); - public PositionVoltage voltageRequest; + setpoint = new State(getPositionRad(), 0.0); + lastGoalRad = setpoint.position; - /* ---------------- Gains ---------------- */ + if (RobotBase.isSimulation()) { + simState = motor.getSimState(); + turretSim = new SingleJointedArmSim( + edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1), + GEAR_RATIO, + 0.01, + 0.15, + MIN_ANGLE_RAD, + MAX_ANGLE_RAD, + false, + 0.0); + } - private static final double kP = 3500.0; + SmartDashboard.putData("Turret Mech", mech); + } - private static final double kD = 150.0; + /* ---------------- Public API ---------------- */ - /* ---------------- Visualization ---------------- */ + public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { + goalAngle = angle; + goalVelocityRadPerSec = velocityRadPerSec; + } - private final Mechanism2d mech = new Mechanism2d(100, 100); - private final MechanismRoot2d root = mech.getRoot("turret", 50, 50); - private final MechanismLigament2d ligament = - root.append(new MechanismLigament2d("barrel", 30, 0)); + public boolean atGoal() { + return Math.abs(setpoint.position - lastGoalRad) < Units.degreesToRadians(1.5); + } - /* ---------------- Constructor ---------------- */ + public double getPositionRad() { + return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; + } - public Turret() { - motor.setNeutralMode(NeutralModeValue.Coast); - motor.setPosition(0.0); + /* ---------------- Periodic ---------------- */ - motor.getConfigurator().apply( - new Slot0Configs() - .withKP(kP) - .withKD(kD)); + @Override + public void periodic() { - motor.getConfigurator().apply( - new com.ctre.phoenix6.configs.TalonFXConfiguration() {{ - MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - }}); - - motor.getConfigurator().apply( - new TalonFXConfiguration() {{ - Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - }} - ); - - voltageRequest = new PositionVoltage(0); + double robotRelativeGoal = goalAngle.getRadians(); - setpoint = new State(getPositionRad(), 0.0); - lastGoalRad = setpoint.position; + // --- MA-style continuous wrap selection --- + double best = lastGoalRad; + boolean found = false; - if (RobotBase.isSimulation()) { - simState = motor.getSimState(); - turretSim = - new SingleJointedArmSim( - edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1), - GEAR_RATIO, - 0.01, - 0.15, - MIN_ANGLE_RAD, - MAX_ANGLE_RAD, - false, - 0.0); - } + for (int i = -2; i <= 2; i++) { + double candidate = robotRelativeGoal + 2.0 * Math.PI * i; + if (candidate < MIN_ANGLE_RAD || candidate > MAX_ANGLE_RAD) + continue; - SmartDashboard.putData("Turret Mech", mech); - } + if (!found || Math.abs(candidate - lastGoalRad) < Math.abs(best - lastGoalRad)) { + best = candidate; + found = true; + } + } - /* ---------------- Public API ---------------- */ + lastGoalRad = best; - public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { - goalAngle = angle; - goalVelocityRadPerSec = velocityRadPerSec; - } + // --- Profile in MECHANISM SPACE --- + State goalState = new State( + MathUtil.clamp(best, MIN_ANGLE_RAD, MAX_ANGLE_RAD), + goalVelocityRadPerSec); - public boolean atGoal() { - return Math.abs(setpoint.position - lastGoalRad) < Units.degreesToRadians(1.5); - } + setpoint = profile.calculate( + Constants.LOOP_TIME, + setpoint, + goalState); - public double getPositionRad() { - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; - } + // --- Convert to MOTOR SPACE --- + double motorPosRot = Units.radiansToRotations(setpoint.position) * GEAR_RATIO; + + double motorVelRotPerSec = Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO; + + double targetVelocity = velocityPid.calculate( + motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians) * GEAR_RATIO, + setpoint.position * GEAR_RATIO); + + var request = new MotionMagicVelocityVoltage(Units.radiansToRotations(targetVelocity)); + + // --- Position + velocity feedforward (MA-style) --- + motor.setControl(request); + + // --- Visualization --- + ligament.setAngle(Units.radiansToDegrees(getPositionRad())); + + SmartDashboard.putNumber("Turret GoalDeg", + Units.radiansToDegrees(best)); + SmartDashboard.putNumber("Turret SetpointDeg", + Units.radiansToDegrees(setpoint.position)); + SmartDashboard.putNumber("Turret motorPosRot", + Units.radiansToDegrees(motorPosRot)); + SmartDashboard.putNumber("Turret motorVelRotPerSec", + Units.radiansToDegrees(motorVelRotPerSec)); + SmartDashboard.putNumber("Turret Position Deg", + Units.radiansToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO); + + // SmartDashboard.putData("Spin to 90 deg", new + // InstantCommand(()->{setFieldRelativeTarget(new Rotation2d(Math.PI), 0);})); + } - /* ---------------- Periodic ---------------- */ + /* ---------------- Simulation ---------------- */ + + @Override + public void simulationPeriodic() { + turretSim.setInputVoltage(motor.getMotorVoltage().getValueAsDouble()); + turretSim.update(Constants.LOOP_TIME); - @Override - public void periodic() { + simState.setRawRotorPosition( + Units.radiansToRotations(turretSim.getAngleRads()) * GEAR_RATIO); - double robotRelativeGoal = goalAngle.getRadians(); - - // --- MA-style continuous wrap selection --- - double best = lastGoalRad; - boolean found = false; - - for (int i = -2; i <= 2; i++) { - double candidate = robotRelativeGoal + 2.0 * Math.PI * i; - if (candidate < MIN_ANGLE_RAD || candidate > MAX_ANGLE_RAD) continue; - - if (!found || Math.abs(candidate - lastGoalRad) < Math.abs(best - lastGoalRad)) { - best = candidate; - found = true; - } - } - - 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; - - // --- Position + velocity feedforward (MA-style) --- - motor.setControl(voltageRequest.withPosition(Rotations.of(motorPosRot)).withVelocity(RotationsPerSecond.of(motorVelRotPerSec))); - - // --- Visualization --- - ligament.setAngle(Units.radiansToDegrees(getPositionRad())); - - SmartDashboard.putNumber("Turret GoalDeg", - Units.radiansToDegrees(best)); - SmartDashboard.putNumber("Turret SetpointDeg", - Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("Turret motorPosRot", - Units.radiansToDegrees(motorPosRot)); - SmartDashboard.putNumber("Turret motorVelRotPerSec", - Units.radiansToDegrees(motorVelRotPerSec)); - SmartDashboard.putNumber("Turret Position Deg", - Units.radiansToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO); - - // SmartDashboard.putData("Spin to 90 deg", new InstantCommand(()->{setFieldRelativeTarget(new Rotation2d(Math.PI), 0);})); - } - - /* ---------------- Simulation ---------------- */ - - @Override - public void simulationPeriodic() { - turretSim.setInputVoltage(motor.getMotorVoltage().getValueAsDouble()); - turretSim.update(Constants.LOOP_TIME); - - simState.setRawRotorPosition( - Units.radiansToRotations(turretSim.getAngleRads()) * GEAR_RATIO); - - simState.setRotorVelocity( - Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * GEAR_RATIO); - } + simState.setRotorVelocity( + Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * GEAR_RATIO); + } } -- 2.39.5