fixes?????

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 32d6dea8b06722cb0ae24df644d1564cf94072c1..97ff2e6d78829f5d53ea496c2807c550dfeb33b2 100644 (file)
--- 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 07e16d23f7e877a24aa6ec55c49600ec6815516e..702088aaf7eb0bd4de6332df52a5a3dc182666e0 100644 (file)
--- 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