merge both cleanup and new climber code to main

src/main/java/org/ironriders/climb/ClimbCommands.java

@@ -13,11 +13,6 @@ public ClimbCommands(ClimbSubsystem climb) {
         climb.publish("Climb to TARGET", goTo(ClimbConstants.Targets.TARGET));
         climb.publish("Re-zero (TESTING ONLY)", reZero());
     }
-
-    public Command set(ClimbConstants.State state) {
-        return climb
-                .runOnce(() -> climb.set(state));
-    }
 
     public Command goTo(ClimbConstants.Targets limit) {
         return climb
@@ -27,4 +22,8 @@ public Command goTo(ClimbConstants.Targets limit) {
     private Command reZero() {
         return climb.runOnce(() -> climb.reZero());
     }
+
+    public Command getGoalpoint() {
+        return climb.runOnce(() -> climb.getGoal());
+    }
 }

src/main/java/org/ironriders/climb/ClimbConstants.java

@@ -1,17 +1,20 @@
 package org.ironriders.climb;
 
+import java.util.Arrays;
+import java.util.Optional;
+
 public class ClimbConstants {
     public static final int CLIMBER_MOTOR_CAN_ID = 17;
     public static final int CURRENT_LIMIT = 40;
 
     public static final double GEAR_RATIO = 1/100;
 
-    public static final double P = 0;
+    public static final double P = 0.1;
     public static final double I = 0; 
     public static final double D = 0;
 
-    public static final double MAX_ACC = 10;
-    public static final double MAX_VEL = 10;
+    public static final double MAX_ACC = 2; // test velocitys
+    public static final double MAX_VEL = 2;
 
     public static final double T = .02;
 
@@ -28,11 +31,14 @@ public enum State {
     public enum Targets {
         HOME(0),
         MAX(-123.5),
-        TARGET(2); //TEST and figure out (2 should be safe and visible) (This is supposed to be the angle where the robot is off the ground but not touching the chain)
+        TARGET(40); //TEST and figure out (40 should be safe and visible) (This is supposed to be the angle where the robot is off the ground but not touching the chain)
 
         public final double pos;
 
         Targets(double pos) { this.pos = pos; }
 
+        public static Optional<Targets> findByCode(Double code) {
+            return Arrays.stream(values()).filter(target -> target.pos == code).findFirst();
+        }   
     }
 }

src/main/java/org/ironriders/climb/ClimbSubsystem.java

@@ -1,5 +1,7 @@
 package org.ironriders.climb;
 
+import java.util.Optional;
+
 import org.ironriders.lib.IronSubsystem;
 
 import com.revrobotics.RelativeEncoder;
@@ -27,9 +29,13 @@ public class ClimbSubsystem extends IronSubsystem {
     private final ClimbCommands commands;
     private final RelativeEncoder encoder;
 
+    private double pidOutput;
+
     private TrapezoidProfile.State goalSetpoint = new TrapezoidProfile.State();
     private TrapezoidProfile.State periodicSetpoint = new TrapezoidProfile.State();
 
+    private SoftLimitConfig softLimitConfig = new SoftLimitConfig(); // should force stop motor if it gets out of bounds
+
     public ClimbSubsystem() {
         publish("Climber P", ClimbConstants.P);
         publish("Climber I", ClimbConstants.I);
@@ -38,14 +44,13 @@ public ClimbSubsystem() {
         encoder = climbMotor.getEncoder();
         encoder.setPosition(0); // Set pos to zero on deploy
 
-        var softLimitConfig = new SoftLimitConfig(); // should force stop motor if it gets out of bounds
         softLimitConfig
                 .reverseSoftLimitEnabled(true)
                 .reverseSoftLimit(ClimbConstants.Targets.MAX.pos)
                 .forwardSoftLimitEnabled(true)
-                .forwardSoftLimit(ClimbConstants.Targets.HOME.pos);
+                .forwardSoftLimit(ClimbConstants.Targets.HOME.pos); // Home is also the minimum position
 
-        climbMotorConfig.idleMode(IdleMode.kBrake);
+        climbMotorConfig.idleMode(IdleMode.kCoast); // for testing set to coast 
         climbMotorConfig.smartCurrentLimit(ClimbConstants.CURRENT_LIMIT);
         climbMotor.configure(climbMotorConfig.apply(softLimitConfig), ResetMode.kResetSafeParameters,
                 PersistMode.kPersistParameters);
@@ -71,9 +76,12 @@ public void periodic() {
         periodicSetpoint = profile.calculate(ClimbConstants.T, periodicSetpoint, goalSetpoint);
 
         publish("Climber set postion", periodicSetpoint.position);
-        publish("Climb Motor Val", getPostion());
+        //publish("Climb Motor Val", getPostion());
+        publish("Climb Motor Val", encoder.getPosition()); // for testing
+
         publish("Climber target pos", goalSetpoint.position);
         publish("Climber target velo", goalSetpoint.velocity);
+        publish("Climber PID output", pidOutput);
 
         pidController.setP(SmartDashboard.getNumber("Climber P", ClimbConstants.P));
         pidController.setI(SmartDashboard.getNumber("Climber I", ClimbConstants.I));
@@ -93,11 +101,20 @@ public void set(ClimbConstants.State state) {
     public void goTo(ClimbConstants.Targets limit) {
         setGoal(limit);
 
-        double pidOutput = pidController.calculate(getPostion() /* Encoder pos times motor gearing */,
+        pidOutput = pidController.calculate(getPostion() /* Encoder pos times motor gearing */,
                 periodicSetpoint.position);
 
+        if (pidOutput == 0) {
+            climbMotor.stopMotor();
+                return;
+        }
+
         climbMotor.set(pidOutput);
-        publish("Climber PID output", pidOutput);
+
+    }
+
+    public double getGoal() {
+        return goalSetpoint.position;
     }
 
     public void setGoal(ClimbConstants.Targets limit) {

src/main/java/org/ironriders/core/RobotCommands.java

@@ -6,7 +6,6 @@
 import org.ironriders.drive.DriveCommands;
 import org.ironriders.elevator.ElevatorCommands;
 import org.ironriders.elevator.ElevatorConstants;
-import org.ironriders.lib.GameState;
 import org.ironriders.targeting.TargetingCommands;
 import org.ironriders.wrist.algae.AlgaeIntakeCommands;
 import org.ironriders.wrist.algae.AlgaeIntakeConstants;
@@ -107,7 +106,7 @@ public Command rumble() {
 
 	public Command toggleClimber() {
 		return Commands.none();
-		// TODO
+		//return climbCommands.goTo(Targets.TARGET);
 	}
 
 	public Command prepareToScoreCoral(ElevatorConstants.Level level) {