package frc.robot.commands.gpm;
import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
private Drivetrain drivetrain;
private TurretVision turretVision;
- double fieldAngleRad;
- double turretSetpoint;
- double adjustedSetpoint;
- double yawToTagCamera;
- double yawToTag;
+ private double fieldAngleRad;
+ private double turretSetpoint;
+ private double adjustedSetpoint;
+ private double yawToTagCamera;
+ private double yawToTag;
private boolean turretVisionEnabled = false;
private boolean SOTM = true;
Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
double D_y;
double D_x;
+
+ // If the robot is moving, adjust the target position based on velocity
if (SOTM) {
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
double xVel = fieldRelVel.vxMetersPerSecond;
double yVel = fieldRelVel.vyMetersPerSecond;
- D_y = target.getY() - drivepose.getY() - (0.92) * yVel;
- D_x = target.getX() - drivepose.getX() - (0.92) * xVel;
+ D_y = target.getY() - drivepose.getY() - (0.67) * yVel;
+ D_x = target.getX() - drivepose.getX() - (0.67) * xVel;
} else {
D_y = target.getY() - drivepose.getY();
D_x = target.getX() - drivepose.getX();
}
+
+ // Calculate the field-relative angle
fieldAngleRad = Math.atan2(D_y, D_x);
- double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Add 180 because drivetrain is backwards
- turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0,180.0);
+
+ // Calculate robot heading and adjust for reverse drive
+ double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment
+
+ // Calculate turret setpoint (angle relative to robot heading)
+ turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
}
public void adjustWithTurretCam(){
if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){
+ // Adjust turret setpoint based on vision input
if(Robot.getAlliance() == Alliance.Blue){
yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get());
}
}
public void updateYawToTag(){
+ // Calculate the yaw offset to the tag
double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY();
double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX();
double angleToTag = Units.radiansToDegrees(Math.atan(D_y / D_x));
@Override
public void initialize() {
+ // Initialize setpoint calculation and set the initial goal for the turret
updateTurretSetpoint();
- turret.setSetpoint(turretSetpoint, drivetrain.getAngularRate(2));
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2));
}
@Override
public void execute() {
+ // Continuously update setpoints and adjust based on vision if available
updateTurretSetpoint();
updateYawToTag();
- if(turretVision != null && turretVisionEnabled && turret.atSetPoint()){
+
+ if(turretVision != null && turretVisionEnabled && turret.atGoal()){
adjustWithTurretCam();
- turret.setSetpoint(adjustedSetpoint, -drivetrain.getAngularRate(2));
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(adjustedSetpoint)), -drivetrain.getAngularRate(2));
} else{
- turret.setSetpoint(turretSetpoint, -drivetrain.getAngularRate(2));
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
}
}
@Override
public void end(boolean interrupted) {
- turret.setSetpoint(0, 0);
+ // Set the turret to a safe position when the command ends
+ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
}
-
-
}
package frc.robot.subsystems.turret;
-import org.littletonrobotics.junction.AutoLogOutput;
-
import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.sim.TalonFXSimState;
import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
+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.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
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;
-import frc.robot.subsystems.shooter.ShooterConstants;
-public class Turret extends SubsystemBase implements TurretIO{
- final private TalonFX motor;
- // Enable here: BUT PROB wont use it
- private boolean infiniteRotation = false;
- private double versaPlanetaryGearRatio = 5.0;
- private double turretGearRatio = 140.0/10.0;
- private final double gearRatio = versaPlanetaryGearRatio * turretGearRatio;
- double power;
+public class Turret extends SubsystemBase {
+ /* ---------------- Constants ---------------- */
+
+ private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180.0);
+ private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180.0);
+
+ private static final double MAX_VEL_RAD_PER_SEC = Units.degreesToRadians(360);
+ private static final double MAX_ACCEL_RAD_PER_SEC2 = Units.degreesToRadians(720);
+
+ 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 PIDController pid = new PIDController(0.2, 0.0, 0.05);
+ /* ---------------- Hardware ---------------- */
+ private final TalonFX motor;
+ private TalonFXSimState simState;
private SingleJointedArmSim turretSim;
- private static final DCMotor turretMotorSim = DCMotor.getKrakenX60(1);
- private TalonFXSimState encoderSim;
- private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * gearRatio); // gear ratio
- private double setpoint = 0;
- Mechanism2d mechanism2d = new Mechanism2d(100, 100);
- MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50);
- MechanismLigament2d ligament2d = mechanismRoot.append(new MechanismLigament2d("turret_motor", 25, 0));
- private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
+ private final MotionMagicVoltage mmRequest = new MotionMagicVoltage(0);
+
+ /* ---------------- Profiling ---------------- */
+
+ private 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;
+
+ /* ---------------- Visualization ---------------- */
+
+ 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));
+
+ /* ---------------- Tuning ---------------- */
- private double dV = 1.0;
private double kP = 15.0;
- private double kI = 0.0;
private double kD = 0.0;
+ /* ---------------- Constructor ---------------- */
+
public Turret() {
- motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN); // switch of course
-
+ motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN);
+ motor.setNeutralMode(NeutralModeValue.Brake);
+
+ TalonFXConfiguration cfg = new TalonFXConfiguration();
+ cfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+ cfg.Slot0.kP = kP;
+ cfg.Slot0.kD = kD;
+
+ MotionMagicConfigs mm = cfg.MotionMagic;
+ mm.MotionMagicCruiseVelocity =
+ Units.radiansToRotations(MAX_VEL_RAD_PER_SEC) * GEAR_RATIO;
+ mm.MotionMagicAcceleration =
+ Units.radiansToRotations(MAX_ACCEL_RAD_PER_SEC2) * GEAR_RATIO;
+
+ motor.getConfigurator().apply(cfg);
+
if (RobotBase.isSimulation()) {
- encoderSim = motor.getSimState();
- turretSim = new SingleJointedArmSim(
- turretMotorSim,
- gearRatio, // gear ratio needs to change
- 0.01 * 0.01 * 5,
- 0.10,
- Units.degreesToRadians(-360),
- Units.degreesToRadians(360),
- false,
- 0.0 // Start angle
- );
+ 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);
}
- motor.setPosition(Units.degreesToRotations(0) * gearRatio); // gear ratio
- motor.setNeutralMode(NeutralModeValue.Brake);
-
- TalonFXConfiguration config = new TalonFXConfiguration();
-
- config.Slot0.kS = 0.1; // Static friction compensation (should be >0 if friction exists)
- config.Slot0.kG = 0.0; // Gravity compensation
- config.Slot0.kV = 0.0; // Velocity gain: 1 rps -> 0.12V
- config.Slot0.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters)
-
- config.Slot0.kP = kP; // If position error is 1 rotation, apply 10V
- config.Slot0.kI = kI; // Integral term (usually left at 0 for MotionMagic)
- config.Slot0.kD = kD; // Derivative term (used to dampen oscillations)
-
- MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY / TurretConstants.TURRET_RADIUS) * gearRatio; // max velocity * gear ratio
- motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION / TurretConstants.TURRET_RADIUS) * gearRatio; // max Acceleration * gear ratio
-
- config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
- motor.getConfigurator().apply(config);
-
- // config.ClosedLoopGeneral.ContinuousWrap = true;
-
- motor.getConfigurator().apply(config);
-
- config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
- config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.degreesToRotations(360) * gearRatio; // max angle * gear ratio
-
- config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;
- config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.degreesToRotations(0) * gearRatio; // min angle * gear ratio
-
- SmartDashboard.putData("turret", mechanism2d);
- SmartDashboard.putData("PID", pid);
-
- SmartDashboard.putData("Set to 0 degrees", new InstantCommand(() -> setSetpoint(0, 0)));
- SmartDashboard.putData("Set to 90 degrees", new InstantCommand(( )-> setSetpoint(90, 0)));
- SmartDashboard.putData("Set to 180 degrees", new InstantCommand(() -> setSetpoint(180, 0)));
- SmartDashboard.putData("Set to -180 degrees", new InstantCommand(() -> setSetpoint(-180, 0)));
- SmartDashboard.putData("Set to -90 degrees", new InstantCommand(() -> setSetpoint(-90, 0)));
- SmartDashboard.putData("Reset Yaw", new InstantCommand(() -> resetYaw()));
- SmartDashboard.putNumber("Shooter Velocity", ShooterConstants.SHOOTER_VELOCITY);
+ SmartDashboard.putData("Turret Mech", mech);
}
- public double getPosition() {
- return inputs.positionDeg;
- }
+ /* ---------------- Public API ---------------- */
- public void resetYaw() {
- inputs.positionDeg = 0;
+ public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
+ goalAngle = angle;
+ goalVelocityRadPerSec = velocityRadPerSec;
}
- public boolean atSetPoint(){
- return Math.abs(getPosition() - setpoint) < 3.0;
+ public boolean atGoal() {
+ return Math.abs(setpoint.position - lastGoalRad) < Units.degreesToRadians(1.5);
}
- public void setSetpoint(double setpointDegrees, double robotRotVel) {
- setpoint = MathUtil.clamp(setpointDegrees, TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE);
-
- if (infiniteRotation) {
- // Get current position in degrees
- double currentDegrees = (motor.getPosition().getValueAsDouble() / gearRatio) * 360.0;
- // Calculate the error
- double error = setpoint - currentDegrees;
- // Wrap the error to [-180, 180]
- // This finds the "remainder" of the distance relative to a full circle
- double optimizedError = Math.IEEEremainder(error, 360.0);
- // Calculate new target in degrees
- double optimizedSetpointDegrees = currentDegrees + optimizedError;
-
- double motorTargetRotations = (optimizedSetpointDegrees / 360.0) * gearRatio;
- motor.setControl(voltageRequest.withPosition(motorTargetRotations));
- } else {
- // normal limited 0,360
- double motorTargetRotations = (setpoint / 360.0) * gearRatio;
-
- //Tune this with rotating robot
- motor.setControl(voltageRequest.withPosition(motorTargetRotations).withFeedForward(dV * robotRotVel));
- }
+ public double getPositionRad() {
+ return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
}
+ /* ---------------- Periodic ---------------- */
+
@Override
- public void periodic() {
- updateInputs();
- ligament2d.setAngle(getPosition());
+ public void periodic() {
+ double robotRelativeGoal = goalAngle.getRadians();
- dV = SmartDashboard.getNumber("FF Constant", dV);
- kP = SmartDashboard.getNumber("kP Value", kP);
- kI = SmartDashboard.getNumber("kI Value", kI);
- kD = SmartDashboard.getNumber("kD Value", kD);
+ // MA-style continuous optimization
+ double best = lastGoalRad;
+ boolean found = false;
- SmartDashboard.putNumber("Turret Position Degrees", getPosition());
- SmartDashboard.putNumber("FF Constant", dV);
- SmartDashboard.putNumber("kP Value", kP);
- SmartDashboard.putNumber("kI Value", kI);
- SmartDashboard.putNumber("kD Value", kD);
+ for (int i = -2; i <= 2; i++) {
+ double candidate = robotRelativeGoal + Math.PI * 2 * 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;
+ }
+ }
- ShooterConstants.SHOOTER_VELOCITY = SmartDashboard.getNumber("Shooter Velocity", ShooterConstants.SHOOTER_VELOCITY);
-
- }
+ lastGoalRad = best;
- @Override
- public void updateInputs(){
- inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / gearRatio;
- inputs.velocity = Units.rotationsPerMinuteToRadiansPerSecond(motor.getVelocity().getValueAsDouble() * 60);
- inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
- }
+ State goal =
+ new State(
+ MathUtil.clamp(best, MIN_ANGLE_RAD, MAX_ANGLE_RAD),
+ goalVelocityRadPerSec);
- public double getAppliedVoltage() {
- return motor.getMotorVoltage().getValueAsDouble();
- }
+ setpoint =
+ profile.calculate(
+ Constants.LOOP_TIME,
+ setpoint,
+ goal);
+
+ double motorRot =
+ Units.radiansToRotations(setpoint.position) * GEAR_RATIO;
- @AutoLogOutput(key = "Turret/SetpointDeg")
- public double getSetpoint(){
- return setpoint;
+ motor.setControl(mmRequest.withPosition(motorRot));
+
+ ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
+
+ SmartDashboard.putNumber("Turret Pos Deg", Units.radiansToDegrees(getPositionRad()));
+ SmartDashboard.putNumber("Turret Goal Deg", Units.radiansToDegrees(best));
}
+ /* ---------------- Simulation ---------------- */
+
@Override
public void simulationPeriodic() {
- //double voltsMotor = power * 12;
- double voltsMotor = motor.getMotorVoltage().getValueAsDouble();
- turretSim.setInputVoltage(voltsMotor);
-
+ turretSim.setInputVoltage(motor.getMotorVoltage().getValueAsDouble());
turretSim.update(Constants.LOOP_TIME);
- double simAngle = turretSim.getAngleRads();
- double simRotations = Units.radiansToRotations(simAngle);
- double motorRotations = simRotations * gearRatio;
+ double motorRot =
+ Units.radiansToRotations(turretSim.getAngleRads()) * GEAR_RATIO;
- encoderSim.setRawRotorPosition(motorRotations);
- encoderSim.setRotorVelocity(turretSim.getVelocityRadPerSec() * Units.radiansToRotations(1) * gearRatio); // Gear Ratio
+ simState.setRawRotorPosition(motorRot);
+ simState.setRotorVelocity(
+ Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * GEAR_RATIO);
}
-}
\ No newline at end of file
+}