Finish climb

src/main/java/com/stuypulse/robot/commands/climb/ClimbToState.java

@@ -1,9 +1,8 @@
 package com.stuypulse.robot.commands.climb;
 
+import com.stuypulse.robot.subsystems.climber.Climb;
 import com.stuypulse.robot.subsystems.climber.Climb.ClimbState;
 
-import com.stuypulse.robot.subsystems.climber.*;
-
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public abstract class ClimbToState extends InstantCommand{

src/main/java/com/stuypulse/robot/constants/Constants.java

@@ -6,9 +6,7 @@
 public interface Constants {
     public interface Climb {
         Rotation2d MIN_ANGLE = Rotation2d.kZero;
-        Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(0.0);
-
-        Rotation2d CLIMBER_OFFSET = Rotation2d.fromDegrees(0.0 - 0.0);
+        Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(87.5);
     }
 
     public interface Drivetrain {

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -27,7 +27,7 @@ public interface PivotConfig {
     }
 
     public interface ClimbConfig {
-        SparkBaseConfig CLIMB_MOTOR_CONFIG = new SparkMaxConfig().smartCurrentLimit(Settings.Climb.CLIMB_CURRENT);
+        SparkBaseConfig CLIMB_MOTOR_CONFIG = new SparkMaxConfig().smartCurrentLimit(Settings.Climb.CLIMB_CURRENT_LIMIT).idleMode(IdleMode.kBrake);
     }
 
     public interface DrivetrainConfig {

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -19,8 +19,7 @@
  */
 public interface Settings {
     public interface LEDPatterns {
-        // Auton Colors
-
+        // AUTON COLORS    
         // MISC
         LEDPattern DO_NOTHING_AUTON = LEDPattern.solid(Color.kWheat).blink(Units.Seconds.of(1.5));
         LEDPattern MOBILITY_AUTON = LEDPattern.solid(Color.kHoneydew).blink(Units.Seconds.of(1.5));
@@ -51,17 +50,20 @@ public interface LEDPatterns {
     }
 
     public interface Climb {
-        Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(0);
-        Rotation2d CLIMBED_ANGLE = Rotation2d.fromDegrees(180);
+        public static final Rotation2d DEFAULT_ANGLE = Rotation2d.fromDegrees(0);
+        public static final Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(0);
+        public static final Rotation2d CLIMBED_ANGLE = Rotation2d.fromDegrees(85);
+
+        public static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(2);
 
-        Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5);
+        public static final int CLIMB_CURRENT_LIMIT = 60;
 
-        int CLIMB_CURRENT = 0;
+        public static final double DEFAULT_VOLTAGE = 0.0;
+        public static final double STOW_VOLTAGE = -1.0;
+        public static final double CLIMBING_VOLTAGE = 1.0;
 
-        double STOW_VOLTAGE = 0.0;
-        double EXTEND_VOLTAGE = 0.0;
-        double CLIMBING_VOLTAGE = 0.0;
-        double DEFAULT_VOLTAGE = 0.0;
+        public static final double CLIMB_MOTOR_GEAR_RATIO = 1.0/100.0;
+        public static final double CLIMB_MOTOR_REDUCTION_FACTOR = 1.0/8.0; //Found from testing
     }
 
     public interface Pivot {
@@ -88,11 +90,12 @@ public interface Pivot {
         public static final String CTRLMODE_MANUAL = "MANUAL";
         public static final String CTRLMODE_STATES = "USING_STATES";
 
-        Rotation2d DEFAULT_ANGLE = Rotation2d.fromDegrees(0);
-        Rotation2d CORAL_STOW_ANGLE = Rotation2d.fromDegrees(-3); 
-        Rotation2d ALGAE_HOLDING_ANGLE = Rotation2d.fromDegrees(-45);
-        Rotation2d ALGAE_INTAKE_ANGLE = Rotation2d.fromDegrees(-70);
-        Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(-85);
+        public static final Rotation2d DEFAULT_ANGLE = Rotation2d.kZero;
+        public static final Rotation2d CORAL_STOW_ANGLE = Rotation2d.fromDegrees(-3); 
+        public static final Rotation2d ALGAE_HOLDING_ANGLE = Rotation2d.fromDegrees(-45);
+        public static final Rotation2d ALGAE_INTAKE_ANGLE = Rotation2d.fromDegrees(-70);
+        public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(-85);
+
     }
 
     public interface Drivetrain {

src/main/java/com/stuypulse/robot/subsystems/climber/Climb.java

@@ -8,9 +8,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public abstract class Climb extends SubsystemBase{
+public abstract class Climb extends SubsystemBase {
     private static final Climb instance;
-    
+
     static {
         instance = new ClimbImpl();
     }
@@ -20,28 +20,37 @@ public static Climb getInstance() {
     }
 
     public enum ClimbState {
-        STOW(Settings.Climb.STOW_ANGLE),
-        CLIMBING(Settings.Climb.CLIMBED_ANGLE);
+        DEFAULT(Settings.Climb.DEFAULT_ANGLE, Settings.Climb.DEFAULT_VOLTAGE),
+        STOW(Settings.Climb.STOW_ANGLE, Settings.Climb.STOW_VOLTAGE),
+        CLIMBING(Settings.Climb.CLIMBED_ANGLE, Settings.Climb.CLIMBING_VOLTAGE);
 
         private Rotation2d targetAngle;
+        private double targetMotorSpeed;
 
-        private ClimbState(Rotation2d targetAngle) {
+        private ClimbState(Rotation2d targetAngle, double targetMotorSpeed) {
             this.targetAngle = Rotation2d.fromDegrees(
-                SLMath.clamp(targetAngle.getDegrees(),Constants.Climb.MIN_ANGLE.getDegrees(), Constants.Climb.MAX_ANGLE.getDegrees()));
+                    SLMath.clamp(targetAngle.getDegrees(), Constants.Climb.MIN_ANGLE.getDegrees(),
+                            Constants.Climb.MAX_ANGLE.getDegrees()));
+
+            this.targetMotorSpeed = SLMath.clamp(targetMotorSpeed, -1.0, 1.0); // Motor speed can only be between -1 & 1
         }
 
         public Rotation2d getTargetAngle() {
             return this.targetAngle;
         }
+
+        public double getTargetMotorSpeed() {
+            return this.targetMotorSpeed;
+        }
     }
 
     private ClimbState state;
 
     protected Climb() {
-        this.state = ClimbState.STOW;
+        this.state = ClimbState.DEFAULT;
     }
 
-    public ClimbState getState(){
+    public ClimbState getState() {
         return this.state;
     }
 
@@ -50,11 +59,11 @@ public void setState(ClimbState state) {
     }
 
     public abstract Rotation2d getCurrentAngle();
+
     public abstract boolean atTargetAngle();
 
     @Override
     public void periodic() {
         SmartDashboard.putString("Climb/State", state.toString());
-        SmartDashboard.putNumber("Climb Angle", Climb.getInstance().getCurrentAngle().getDegrees());
     }
 }

src/main/java/com/stuypulse/robot/subsystems/climber/ClimbImpl.java

@@ -1,53 +1,61 @@
 package com.stuypulse.robot.subsystems.climber;
 
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
 
 import com.stuypulse.robot.constants.Constants;
-import com.stuypulse.robot.constants.Gains;
+import com.stuypulse.robot.constants.Motors;
+import com.stuypulse.robot.constants.Motors.ClimbConfig;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
-import com.stuypulse.stuylib.control.Controller;
-import com.stuypulse.stuylib.control.feedback.PIDController;
-import com.stuypulse.stuylib.control.feedforward.ArmFeedforward;
-import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
-import com.stuypulse.robot.constants.Motors.ClimbConfig;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class ClimbImpl extends Climb {
     private SparkMax climbMotor;
     private RelativeEncoder climbEncoder;
 
-    private Controller controller;
-
     public ClimbImpl() {
         super();
         climbMotor = new SparkMax(Ports.Climb.CLIMB_MOTOR, MotorType.kBrushless);
-        climbEncoder = climbMotor.getEncoder();
-        climbMotor.configure(ClimbConfig.CLIMB_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
 
-        controller = new MotorFeedforward(Gains.Climb.FF.kS, Gains.Climb.FF.kV, Gains.Climb.FF.kA).position()
-            .add(new ArmFeedforward(Gains.Climb.FF.kG))
-            .add(new PIDController(Gains.Climb.PID.kP, Gains.Climb.PID.kI, Gains.Climb.PID.kD));
+        Motors.ClimbConfig.CLIMB_MOTOR_CONFIG.encoder
+                .positionConversionFactor(
+                        Settings.Climb.CLIMB_MOTOR_GEAR_RATIO * Settings.Climb.CLIMB_MOTOR_REDUCTION_FACTOR);
+
+        climbMotor.configure(ClimbConfig.CLIMB_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters,
+                PersistMode.kNoPersistParameters);
+        climbEncoder = climbMotor.getEncoder();
     }
 
     @Override
-    public Rotation2d getCurrentAngle(){
-        return Rotation2d.fromRotations(climbEncoder.getPosition() - Constants.Climb.CLIMBER_OFFSET.getRotations());
+    public Rotation2d getCurrentAngle() {
+        return Rotation2d.fromRotations(climbEncoder.getPosition());
     }
 
     @Override
     public boolean atTargetAngle() {
-        return (getState().getTargetAngle().getDegrees() - getCurrentAngle().getDegrees()) > Settings.Climb.ANGLE_TOLERANCE.getDegrees();
+        return Math.abs(getCurrentAngle().getDegrees()
+                - getState().getTargetAngle().getDegrees()) < Settings.Climb.ANGLE_TOLERANCE.getDegrees();
     }
 
     @Override
-    public void periodic(){
+    public void periodic() {
         super.periodic();
-        climbMotor.setVoltage(controller.update(getState().getTargetAngle().getDegrees(), getCurrentAngle().getDegrees()));
+
+        if (!atTargetAngle()) {
+            climbMotor.set(getState().getTargetMotorSpeed());
+        } else {
+            climbMotor.set(0.0);
+        }
+
+        SmartDashboard.putNumber("Climb/Angular Velocity", climbEncoder.getVelocity());
+        SmartDashboard.putNumber("Climb/Current Angle", getCurrentAngle().getDegrees());
+        SmartDashboard.putNumber("Climb/Target angle", getState().getTargetAngle().getDegrees());
+        SmartDashboard.putBoolean("Climb/At target angle", atTargetAngle());
     }
 }

src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java

@@ -35,11 +35,6 @@ public static Drivetrain getInstance() {
 
     public abstract Pose2d getPose();
 
-
-
-
-
-
     @Override
     public void periodic() {}
 }