From: Wesley28w Date: Sat, 17 Jan 2026 22:28:27 +0000 (-0800) Subject: gearing X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=0390d3109dc4625c39fd42bd4b327545dd31e88c;p=FRC2026.git gearing --- diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 74d19c2..25c68e7 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,12 +1,19 @@ -package frc.robot.subsystems.Turret; +package frc.robot.subsystems.turret; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +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.system.plant.DCMotor; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DigitalInput; +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; @@ -18,150 +25,149 @@ import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; public class Turret extends SubsystemBase { - private double target_x = 1; - private double target_y = 0; - private TalonFX motor; - private final DigitalInput sensor = new DigitalInput(0); - double power; - /** direction the turet is pointing in degrees */ - private double position; - /** how fast the turret is moving in radians per second */ - private double velocity; - private PIDController pid = new PIDController(0.002, 0, 0); - private boolean sensorTriggered; - - private boolean motorCalibrated; - + double D_x = 1; + double D_y = 1; + final private TalonFX motor; private double versaPlanetaryGearRatio = 5.0; - /** 140 teeth divided by 10 teeth */ private double turretGearRatio = 140.0/10.0; private final double gearRatio = versaPlanetaryGearRatio * turretGearRatio; - private double calibrationOffset = 0; - + + private double position; + private double velocity; + double power; + + private PIDController pid = new PIDController(0.2, 0.0, 0.05); + private SingleJointedArmSim turretSim; - private static final DCMotor simMotor = DCMotor.getKrakenX60(1); + private static final DCMotor turretMotorSim = DCMotor.getKrakenX60(1); private TalonFXSimState encoderSim; - + private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(Units.degreesToRotations(0) * 1); // 1 is placeholder for 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", 25, 0)); + MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50); + MechanismLigament2d ligament2d = mechanismRoot.append(new MechanismLigament2d("turret_motor", 25, 0)); public Turret() { - motor = new TalonFX(IdConstants.TURRET_MOTOR_ID); - encoderSim = motor.getSimState(); - pid.setTolerance(Units.degreesToRadians(2)); - sensorTriggered = false; - motorCalibrated = false; - - turretSim = new SingleJointedArmSim( - simMotor, - turretGearRatio, - 0.1, - 0.3, - 0, - Units.degreesToRadians(360), - false, - Units.degreesToRadians(-360) - ); + motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_CAN); // switch of course - SmartDashboard.putData("turret", mechanism2d); - SmartDashboard.putData("PID", pid); - SmartDashboard.putBoolean("Calibrated", motorCalibrated); + 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 + ); + } - SmartDashboard.putData("Set 90 degrees", new InstantCommand(() -> spinTo(90))); - SmartDashboard.putData("Set 180 degrees", new InstantCommand(() -> spinTo(180))); - SmartDashboard.putData("Set 0 degrees", new InstantCommand(() -> spinTo(0))); - SmartDashboard.putData("Set 270 degrees", new InstantCommand(() -> spinTo(270))); + motor.setPosition(Units.degreesToRotations(0) * gearRatio); // gear ratio + motor.setNeutralMode(NeutralModeValue.Brake); - SmartDashboard.putData("Set target (1,1)", new InstantCommand(() -> setTarget(1, 1))); - SmartDashboard.putData("Set target (-1,1)", new InstantCommand(() -> setTarget(-1, 1))); - SmartDashboard.putData("Set target (-1,-1)", new InstantCommand(() -> setTarget(-1, -1))); - SmartDashboard.putData("Set target (1,-1)", new InstantCommand(() -> setTarget(1, -1))); + TalonFXConfiguration config = new TalonFXConfiguration(); + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = 40; + limitConfig.StatorCurrentLimitEnable = true; - pid.enableContinuousInput(-Math.PI, Math.PI); + motor.getConfigurator().apply(limitConfig); - } + // to be frank I just took this from hood because I don't know good values yet + config.Slot0.kS = 0.1; // Static friction compensation (should be >0 if friction exists) + config.Slot0.kG = 0.25; // Gravity compensation + config.Slot0.kV = 0.12; // Velocity gain: 1 rps -> 0.12V + config.Slot0.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters) + + config.Slot0.kP = Units.radiansToRotations(3.0 * 12); // If position error is 2.5 rotations, apply 12V (0.5 * 2.5 * 12V) + config.Slot0.kI = Units.radiansToRotations(0.00); // Integral term (usually left at 0 for MotionMagic) + config.Slot0.kD = Units.radiansToRotations(0.00 * 12); // Derivative term (used to dampen oscillations) + + MotionMagicConfigs motionMagicConfigs = config.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = Units.degreesToRotations(0.1) * gearRatio; // max velocity * gear ratio + motionMagicConfigs.MotionMagicAcceleration = Units.degreesToRotations(0.1) * gearRatio; // max Acceleration * gear ratio - /** position in degrees of the turret not the turret motor */ - public double getPosition(){ - return position; - } + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + motor.getConfigurator().apply(config); - public void setTarget(double x, double y) { - target_x = x; - target_y = y; - } + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.degreesToRotations(360) * gearRatio; // max angle * gear ratio + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.degreesToRotations(0) * gearRatio; // min angle * gear ratio + + SmartDashboard.putData("turret", mechanism2d); + SmartDashboard.putData("PID", pid); + + SmartDashboard.putData("Set to 1,1", new InstantCommand(() -> setTarget(1,1))); + SmartDashboard.putData("Set to -1,1", new InstantCommand(( )-> setTarget(-1,1))); + SmartDashboard.putData("Set to -1,-1", new InstantCommand(() -> setTarget(-1,-1))); + SmartDashboard.putData("Set to 1,-1", new InstantCommand(() -> setTarget(1,-1))); - /** - * set the target angle - * @param setPoint angle in degrees - */ - public void spinTo(double setPoint){ - pid.reset(); - pid.setSetpoint(Units.degreesToRadians(setPoint)); + SmartDashboard.putData("Print out target position", new InstantCommand(()-> System.out.println(getTargetPosition()))); } - public boolean atSetPoint(){ - return pid.atSetpoint(); + public void setTarget(double x, double y) { + D_x = x; + D_y = y; } - public double getVelocity(){ - return velocity/gearRatio; + public double[] getTargetPosition() { + System.out.println("Distance X value is: "+ D_x + "and the Distance Y valye is: " + D_y); + double[] target = {D_x, D_y}; + return target; } - public boolean isSensorTriggered(){ - return sensorTriggered; + public double getPosition() { + return motor.getPosition().getValueAsDouble() / gearRatio; // Gear ratio } - public boolean isMotorCalibrated(){ - return motorCalibrated; + public void setSetpoint(double setpointDegrees) { + // If you want to handle the wrap-around ( -180 to 180) + double clampedSetpoint = MathUtil.inputModulus(setpointDegrees, -180, 180); + this.setpoint = clampedSetpoint; + + // Convert mechanism degrees to motor rotations: (Degrees / 360) * GearRatio + double motorTargetRotations = (clampedSetpoint / 360.0) * gearRatio; + motor.setControl(voltageRequest.withPosition(motorTargetRotations)); } - public void calibrate() { - motor.set(0.02); - sensorTriggered = sensor.get(); - if(isSensorTriggered()) { - if(velocity > 0) { - position = 0 - calibrationOffset; - } else { - position = 0 + calibrationOffset; - } - motorCalibrated = true; - } + public void align() { + double angleRad = Math.atan2(D_y, D_x); + setSetpoint(Units.radiansToDegrees(angleRad)); } @Override public void periodic() { - if(!isMotorCalibrated()) { - calibrate(); - } else { - position = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()); - velocity = Units.rotationsPerMinuteToRadiansPerSecond(motor.getVelocity().getValueAsDouble() * 60); - /** feeding the PID radians */ - power = pid.calculate(Units.degreesToRadians(getPosition())); - motor.set(power); - sensorTriggered = sensor.get(); - ligament2d.setAngle(position); - spinTo(Units.radiansToDegrees(Math.atan2(target_y,target_x))); - } + position = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / gearRatio; // Gear Ratio + velocity = Units.rotationsPerMinuteToRadiansPerSecond(motor.getVelocity().getValueAsDouble() * 60); + + // SmartDashboard.putNumber("Turret Position", position); + // align(); + ligament2d.setAngle(position); + } public double getAppliedVoltage() { return motor.getMotorVoltage().getValueAsDouble(); } + boolean simInitialized = false; @Override public void simulationPeriodic() { - double voltsMotor = power * 12; + //double voltsMotor = power * 12; + double voltsMotor = motor.getMotorVoltage().getValueAsDouble(); turretSim.setInputVoltage(voltsMotor); turretSim.update(Constants.LOOP_TIME); double simAngle = turretSim.getAngleRads(); double simRotations = Units.radiansToRotations(simAngle); - double motorRotations = simRotations * gearRatio; - - encoderSim.setRawRotorPosition(motorRotations); + double motorRotations = simRotations * gearRatio; // gear ratio is 1 + + encoderSim.setRawRotorPosition(motorRotations); // MUST set position + encoderSim.setRotorVelocity(turretSim.getVelocityRadPerSec() * Units.radiansToRotations(1) * gearRatio); // Gear Ratio } -} +} \ No newline at end of file