small fixes + smartdashboard

Author: ProfessorAtomicManiac

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
 }
 
 java {

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

@@ -187,14 +187,14 @@ private void setBindingsManipulatorENDEFF() {
       );
       new JoystickButton(manipulatorController, Button.kY.value).onTrue(new MoveToPos(arm, AMP_ANGLE_RAD));
       new JoystickButton(manipulatorController, Button.kA.value).onTrue(new MoveToPos(arm, GROUND_INTAKE_POS));
-      new JoystickButton(manipulatorController, Button.kB.value).onTrue(new moveClimber(arm));
-      new JoystickButton(manipulatorController, Button.kX.value).onTrue(new IntakeTesting(intakeShooter));
+      //new JoystickButton(manipulatorController, Button.kB.value).onTrue(new moveClimber(arm));
+      //new JoystickButton(manipulatorController, Button.kX.value).onTrue(new IntakeTesting(intakeShooter));
         //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 moveClimber(arm));
+      new JoystickButton(manipulatorController, LOWER_CLIMBER).onTrue(new ClimbArmSoftLimit(arm));
 
     //NEW BINDINGS(easier for manipulator)
     //Xbox left joy Y axis -> raw Intake/Outtake control

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

@@ -5,6 +5,11 @@
 import static org.carlmontrobotics.Constants.Armc.*;
 
 import org.carlmontrobotics.Constants.Armc.*;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class ClimbArmSoftLimit extends Command {
@@ -13,26 +18,27 @@ public class ClimbArmSoftLimit extends Command {
     public ClimbArmSoftLimit(Arm arm) {
         this.arm = arm;
         addRequirements(arm);
+        SmartDashboard.putNumber("climber volts", 0);
     }
     @Override
     public void initialize() {
         arm.setLimitsForClimbOn();
-        arm.setArmTarget(CLIMBER_UP_ANGLE_RAD);
-        arm.setBooleanDrive(true);
+        arm.setBooleanDrive(false);
     }
     @Override
     public void execute() {
-    arm.driveArmMax(-1);
-    arm.setSoftLimit( (float) SOFT_LIMIT_LOCATION_IN_RADIANS); //TODO: Find out what limit actually is defined as
+        arm.driveArm(SmartDashboard.getNumber("climber volts", 0));
     }
     @Override
     public void end(boolean interrupted) {
         arm.stopArm(); //OR maintain small voltage to keep arm in place?
         arm.resetSoftLimit();
-        arm.setBooleanDrive(false);
+        arm.setArmTarget(arm.getArmPos());
+        arm.setBooleanDrive(true);
     }
     @Override
     public boolean isFinished() {
-        return arm.getArmPos() <= CLIMBER_DOWN_ANGLE_RAD + 0.02 || arm.getArmPos() >= CLIMBER_DOWN_ANGLE_RAD - 0.05; //TODO: Figure out the actual climb position
+        //TODO: Figure out the actual climb position
+        return Math.abs(arm.getArmPos() - CLIMBER_DOWN_ANGLE_RAD) < Units.degreesToRadians(5);
     }
 }

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

@@ -149,6 +149,9 @@ public Arm() {
 
         lastArmPos = getArmPos();
         lastMeasuredTime = Timer.getFPGATimestamp();
+
+        SmartDashboard.putNumber("ramp rate (s)", 2);
+        SmartDashboard.putNumber("soft limit pos (rad)", SOFT_LIMIT_LOCATION_IN_RADIANS);
     }
     public void setBooleanDrive(boolean climb) {
         callDrive = climb;
@@ -255,20 +258,18 @@ private void driveArm() {
         SmartDashboard.putNumber("pid volts", armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput() - armFeedVolts);
     }
 
-    public void drivearm(double throttleVolts) {
-        armMotorMaster.setVoltage(throttleVolts);
-    }
-
     public void stopArm() {
         armMotorMaster.set(0);
 
     }
-    public void driveArmMax(int direction) {
-       drivearm(6 * direction); //STARTING WITH SLOWER SPEED FOR TESTING
+    public void driveArm(double volts) {
+        //armMotorMaster.set(0)
+       armMotorMaster.setVoltage(volts); //STARTING WITH SLOWER SPEED FOR TESTING
     }
     public void setLimitsForClimbOn() {
         armPIDMaster.setOutputRange(-12, 12);
-        armMotorMaster.setOpenLoopRampRate(5);
+        armMotorMaster.setSoftLimit(SoftLimitDirection.kReverse, (float)SmartDashboard.getNumber("soft limit pos (rad)", SOFT_LIMIT_LOCATION_IN_RADIANS));
+        armMotorMaster.setOpenLoopRampRate(SmartDashboard.getNumber("ramp rate (s)", 2));
         armMotorMaster.enableSoftLimit(SoftLimitDirection.kReverse, true);
     }
     /**
@@ -332,13 +333,10 @@ public double getArmPos() {
     public double getArmVel() {
         return armMasterEncoder.getVelocity();
     }
-    public void setSoftLimit(float limit) {
-        
-        armMotorMaster.setSoftLimit(SoftLimitDirection.kReverse, limit);
-    }
     public void resetSoftLimit() {
-        armPIDMaster.setOutputRange(-12/MIN_VOLTAGE, 12/MAX_VOLTAGE);
+        armPIDMaster.setOutputRange(MIN_VOLTAGE/-12, MAX_VOLTAGE/12);
         armMotorMaster.enableSoftLimit(SoftLimitDirection.kReverse, false);
+        armMotorMaster.setOpenLoopRampRate(0);
     }
     public TrapezoidProfile.State getCurrentArmState() {
         return new TrapezoidProfile.State(getArmPos(), getArmVel());