Climber code

Author: DriverStationComputer

simgui.json

@@ -71,7 +71,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 338
+          "height": 621
         }
       ]
     }

src/main/java/org/carlmontrobotics/Constants.java

@@ -139,6 +139,7 @@ public static final class Armc {
 		public static final double AMP_ANGLE_RAD = 1.28;
 		//-0.427725
 		public static final double GROUND_INTAKE_POS = Units.degreesToRadians(-24.5068373);
+		public static final double HANG_ANGL_RAD = GROUND_INTAKE_POS + Units.degreesToRadians(30);
 
 		public static final double SUBWOOFER_ANGLE_RAD = Units.degreesToRadians(0);//touching the base of the speaker
 		public static final double SAFE_ZONE_ANGLE_RAD = Units.degreesToRadians(36);//touching the white line

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -191,6 +191,9 @@ private void setBindingsManipulatorENDEFF() {
         //new WaitCommand(.5),
         //new MoveToPos(arm, GROUND_INTAKE_POS)
        //MoveToPos(arm, GROUND_INTAKE_POS));
+      
+      new JoystickButton(manipulatorController, RAISE_CLIMBER).onTrue(new MoveToPos(arm, Armc.UPPER_ANGLE_LIMIT_RAD));
+      new JoystickButton(manipulatorController, LOWER_CLIMBER).onTrue(new MoveToPos(arm, Armc.HANG_ANGLE_RAD));
 
     //NEW BINDINGS(easier for manipulator)
     //Xbox left joy Y axis -> raw Intake/Outtake control

src/main/java/org/carlmontrobotics/commands/moveClimber.java

@@ -4,17 +4,14 @@
 
 package org.carlmontrobotics.commands;
 
-import static org.carlmontrobotics.Constants.Armc.GROUND_INTAKE_POS;
-import static org.carlmontrobotics.Constants.Armc.HANG_ANGLE_RAD;
-import static org.carlmontrobotics.Constants.Armc.LOWER_ANGLE_LIMIT_RAD;
-import static org.carlmontrobotics.Constants.Armc.MAX_FF_ACCEL_RAD_P_S;
-import static org.carlmontrobotics.Constants.Armc.TRAP_CONSTRAINTS;
-import static org.carlmontrobotics.Constants.Armc.UPPER_ANGLE_LIMIT_RAD;
+import static edu.wpi.first.units.Units.Volt;
+import static org.carlmontrobotics.Constants.Armc.*;
 
 import org.carlmontrobotics.subsystems.Arm;
 
 import com.revrobotics.CANSparkBase;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -23,7 +20,7 @@
 public class moveClimber extends Command {
   /** Creates a new moveClimber. */
   private final Arm arm;
-  private double goal;
+  private double goal = HANG_ANGL_RAD;
 
   public moveClimber(Arm arm) {
     // Use addRequirements() here to declare subsystem dependencies.
@@ -33,33 +30,35 @@ public moveClimber(Arm arm) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    arm.callDrive = false;
-    if (!arm.armClimbing) {
-      goal = HANG_ANGLE_RAD;
-    } else {
-      goal = GROUND_INTAKE_POS;
-    }
-    arm.armClimbing = !arm.armClimbing;
+    arm.callDrive = false;//turn normal arm periodic off
+    arm.setArmTarget(goal);
   }
 
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    arm.drivearm(Math.signum(goal - arm.getArmPos()));
+    double err = goal - arm.getArmPos();
+    arm.drivearm(//equivalent to a clamped P controller with KS
+      Math.signum(err) * //direction control
+          MathUtil.clamp(
+            Math.abs(err)*2,//.5rad err -> 1 speed
+            .2,
+            .1
+          )
+    );
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    arm.setArmTarget(goal);
+    arm.driveMotor(Volt.of(0));
     arm.callDrive = true;
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-
     return Math.abs(arm.getArmPos() - goal) < .1;
   }
 }