From: Ethan Mortensen Date: Sat, 14 Feb 2026 20:03:31 +0000 (-0800) Subject: fixes????? X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=d1cf1c86dc9193a636ef59a0ef4e7c486af9b212;p=FRC2026.git fixes????? --- diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java index 32d6dea..97ff2e6 100644 --- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java @@ -4,10 +4,10 @@ public class ClimbConstants { // CHANGE LATER public final static double CLIMB_GEAR_RATIO = 9.0 / 1 * 5.0 / 1; - public final static double MAX_VELOCITY = 2; - public final static double MAX_ACCELERATION = 10; - public final static double MIN_HEIGHT = 0; + public final static double MAX_VELOCITY = 1; + public final static double MAX_ACCELERATION = 0.3; + public final static double MIN_HEIGHT = 6; public final static double MAX_HEIGHT = 8; - public final static double RADIUS = 0.514098; + public final static double RADIUS = 0.3; public final static double CLIMB_HEIGHT = 4; } diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java index 07e16d2..702088a 100644 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.Climb; +import static edu.wpi.first.units.Units.Rotation; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -10,21 +12,25 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; +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.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.constants.Climb.ClimbConstants; import frc.robot.constants.swerve.DriveConstants; -public class LinearClimb { +public class LinearClimb extends SubsystemBase{ private TalonFX motor; private double rotationalSetpoint = 0; - private double kP = 1.5; + private static PIDController pid = new PIDController(0.5, 0, 0); + + private double kP = 1.0; private double kI = 0.0; private double kD = 0.0; @@ -43,8 +49,8 @@ public class LinearClimb { public LinearClimb() { motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID);//, Constants.RIO_CAN); + pid.setTolerance(0.2); - motor.setPosition(Units.degreesToRotations(0) * gearRatio); // gear ratio motor.setNeutralMode(NeutralModeValue.Brake); TalonFXConfiguration config = new TalonFXConfiguration(); @@ -52,47 +58,59 @@ public class LinearClimb { config.CurrentLimits = new CurrentLimitsConfigs(); config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = 10.0; + config.CurrentLimits.StatorCurrentLimit = 5.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 10.0; + config.CurrentLimits.SupplyCurrentLimit = 5.0; - config.Slot0.kS = 0.0; // Static friction compensation (should be >0 if friction exists) - config.Slot0.kG = 0.0; // Gravity compensation - config.Slot0.kV = 0.5; // Velocity gain: 1 rps -> 0.12V - config.Slot0.kA = 10.0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters) + // config.Slot0.kS = 0.0; // Static friction compensation (should be >0 if friction exists) + // config.Slot0.kG = 0.0; // Gravity compensation + // config.Slot0.kV = 1; // Velocity gain: 1 rps -> 0.12V + // config.Slot0.kA = 0.3; // 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) + // 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(ClimbConstants.MAX_VELOCITY) * gearRatio; // max velocity * gear ratio - motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(ClimbConstants.MAX_ACCELERATION) * gearRatio; // max Acceleration * gear ratio + // MotionMagicConfigs motionMagicConfigs = config.MotionMagic; + // motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(ClimbConstants.MAX_VELOCITY) * gearRatio; // max velocity * gear ratio + // motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(ClimbConstants.MAX_ACCELERATION) * gearRatio; // max Acceleration * gear ratio + // SmartDashboard.putNumber("velocity", Units.radiansToRotations(ClimbConstants.MAX_VELOCITY) * gearRatio); + // SmartDashboard.putNumber("acceleration", Units.radiansToRotations(ClimbConstants.MAX_ACCELERATION) * gearRatio); config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; motor.getConfigurator().apply(config); SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown())); SmartDashboard.putData("Climb", new InstantCommand(() -> climb())); + SmartDashboard.putNumber("Position", getPosition()); - motor.setPosition((ClimbConstants.MAX_HEIGHT/(2 * Math.PI * ClimbConstants.RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO); + // motor.setPosition((ClimbConstants.MAX_HEIGHT/(2 * Math.PI * ClimbConstants.RADIUS)) * gearRatio); + motor.setPosition(0); } + /** + * set the setpoint for the pid + * @param setpoint in rotations + */ public void setSetpoint(double setpoint) { - setpoint = MathUtil.clamp(setpoint, ClimbConstants.MIN_HEIGHT, ClimbConstants.MAX_HEIGHT); + pid.setSetpoint(setpoint); + // setpoint = MathUtil.clamp(setpoint, ClimbConstants.MIN_HEIGHT, ClimbConstants.MAX_HEIGHT); - rotationalSetpoint = (setpoint / (2 * Math.PI * ClimbConstants.RADIUS)) * gearRatio; + // // rotationalSetpoint = (setpoint / (2 * Math.PI * ClimbConstants.RADIUS)) * gearRatio; + // rotationalSetpoint = -45/2.25; + // System.out.println(rotationalSetpoint); - // motor.setControl(voltageRequest.withPosition(rotationalSetpoint).withFeedForward(feedforward.calculate(0))); - motor.setControl(voltageRequest.withPosition(rotationalSetpoint)); + // // motor.setControl(voltageRequest.withPosition(rotationalSetpoint).withFeedForward(feedforward.calculate(0))); + // motor.setControl(voltageRequest.withPosition(rotationalSetpoint)); } public boolean atSetPoint(){ - return Math.abs(motor.getPosition().getValueAsDouble() - rotationalSetpoint) < 3.0; + return pid.atSetpoint(); + // return Math.abs(motor.getPosition().getValueAsDouble() - rotationalSetpoint) < 3.0; } public double getPosition() { - return (motor.getPosition().getValueAsDouble() / gearRatio) * 2 * Math.PI * ClimbConstants.RADIUS; + return motor.getPosition().getValueAsDouble(); } public void goUp() { @@ -110,6 +128,9 @@ public class LinearClimb { } public void periodic() { - + double power = pid.calculate(motor.getPosition().getValueAsDouble()); + power = MathUtil.clamp(power, -0.2, 0.2); + motor.set(power); + SmartDashboard.putNumber("Position", getPosition()); } } \ No newline at end of file