From: moo Date: Sun, 25 Jan 2026 19:48:37 +0000 (-0800) Subject: MEGA CHARZARD X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=d4beabb549a47ca7a62adeeca0133a3a2d00b3ca;p=FRC2026.git MEGA CHARZARD --- diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java index ccf27c0..1a65db3 100644 --- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java @@ -1,6 +1,7 @@ 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; @@ -17,11 +18,11 @@ public class TurretAutoShoot extends Command { 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; @@ -43,27 +44,36 @@ public class TurretAutoShoot extends Command { 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()); } @@ -76,6 +86,7 @@ public class TurretAutoShoot extends Command { } 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)); @@ -84,26 +95,28 @@ public class TurretAutoShoot extends Command { @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); } - - } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index ecbde7f..1e7b43a 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,7 +1,5 @@ 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; @@ -11,8 +9,9 @@ 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.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; @@ -20,186 +19,165 @@ 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; -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 +}