me and jerry analyzed the climb code

the power need to be flipped!!!
I added logging
Everything else seems okay besides a lot of odd naming and conversions that are a bit excessive. Also some constants are unneeded

diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
index 9a631ff1d3766c6c27b81067500fee7f52b038a3..0f19acdd1115898f4458ebb0f1998a9574d5de90 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
@@ -14,8 +14,8 @@ public class ClimbConstants {
     /** climber stowed ? position in meters */
     public final static double BOTTOM_POSITION = Units.inchesToMeters(8);
     /** Calibration position: lower than BOTTOM_POSITION to reduce motor strain */
-    public final static double CALIBRATION_POSITION = Units.inchesToMeters(8.5);
-    /** position that should put the robot off the ground? in meters. */
+    // public final static double CALIBRATION_POSITION = Units.inchesToMeters(8.5);
+    /** position that should put the robot off the ground? in meters. 6 inches */
     public final static double CLIMB_POSITION = Units.inchesToMeters(6);
     public final static double UP_POSITION = 0.0;
 

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 986c093bb156aaa2b3afa4ebde8258c91400b9da..7422797e5148460ad770cfcbd2fb6e9471085a5d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -38,6 +38,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
     // logging information
     private LinearClimbIOInputsAutoLogged inputs = new LinearClimbIOInputsAutoLogged();
 
+    /** This PID controller uses motor rotations */
     private final PIDController pid = new PIDController(
             ClimbConstants.PID_P,
             ClimbConstants.PID_I,
@@ -55,9 +56,9 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
 
         setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
 
-        SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
-        SmartDashboard.putData("Go Down", new InstantCommand(() -> retract()));
-        SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition()));
+        SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); // 0
+        SmartDashboard.putData("Go Down", new InstantCommand(() -> retract())); // 8
+        SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition())); // 6
 
         // starting position
         motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION));
@@ -110,7 +111,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
     /**
      * goes to the up position
      */
-    public void goUp() {
+    public void goUp() { // 0
         MAX_POWER = 0.8;
         setSetpoint(metersToRotations(ClimbConstants.UP_POSITION));
     }
@@ -118,7 +119,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
     /**
      * goes to the down position
      */
-    public void retract() {
+    public void retract() { // 8
         MAX_POWER = 0.2;
         setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
     }
@@ -126,7 +127,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
     /**
      * goes to the climb position
      */
-    public void climbPosition() {
+    public void climbPosition() { // 6
         MAX_POWER = 0.8;
         setSetpoint(metersToRotations(ClimbConstants.CLIMB_POSITION));
     }
@@ -145,8 +146,9 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
                 stopCalibrating();
             }
         }
-        motor.set(power);
 
+        motor.set(power); // during calibration we have 20ms of high power before we stop calibration
+        SmartDashboard.putNumber("Climb Power from PID", power);
         SmartDashboard.putNumber("Climb PID_Setpoint_Rotations", pid.getSetpoint());
         SmartDashboard.putNumber("Climb Motor_Actual_Rotations", motor.getPosition().getValueAsDouble());
         SmartDashboard.putNumber("Climb Motor_Actual_Meters", inputs.positionMeters);
@@ -222,5 +224,6 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO {
         inputs.positionMeters = Units.rotationsToRadians(motor.getPosition().getValueAsDouble())
                 * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO;
         inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
+        inputs.power = pid.calculate(motor.getPosition().getValueAsDouble());
     }
 }

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java
index 4273b7bf31ab0cdddd79163f568874cfa14ee021..a3ed4e94472b6ec6f9e400b64514a5dd83248d21 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java
@@ -7,6 +7,7 @@ public interface LinearClimbIO {
     public static class LinearClimbIOInputs{
         public double positionMeters = 0.0;
         public double motorCurrent = 0.0;
+        public double power = 0.0;
     }
 
     public void updateInputs();