Added new command and limits for arm | NOT FINISHED PLEASE HELP IM TIRED

Author: aaron345678

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

@@ -165,15 +165,15 @@ public static final class Armc {
 		//fine for now, change it later before use - ("Incorect use of setIZone()" Issue #22)
 		//public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s
 		public static final double MAX_FF_ACCEL_RAD_P_S =  53.728/4; // rad / s^2 ((.89*2)/(1.477/(61.875^2))/61.875)-20.84
-
+		public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0;
 
 		public static final double MIN_VOLTAGE = -0.5; //-((kS + kG + 1)/12);
 		public static final double MAX_VOLTAGE = 0.5; //(kS + kG + 1)/12;
-
+		
 		// if needed
 		public static final double COM_ARM_LENGTH_METERS = 0.381;
 		public static final double ARM_MASS_KG = 9.59302503;
-
+		
 		public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS;//initalized by arm constructor
 		// other0;
 

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

@@ -0,0 +1,38 @@
+package org.carlmontrobotics.commands;
+
+import org.carlmontrobotics.subsystems.Arm;
+
+import static org.carlmontrobotics.Constants.Armc.*;
+
+import org.carlmontrobotics.Constants.Armc.*;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class ClimbArmSoftLimit extends Command {
+    //TODO: Debug this entire thing because it is 1 am and no way i did this correctly
+    private Arm arm;
+    public ClimbArmSoftLimit(Arm arm) {
+        this.arm = arm;
+        addRequirements(arm);
+    }
+    @Override
+    public void initialize() {
+        arm.setLimitsForClimbOn();
+        arm.setArmTarget(CLIMBER_UP_ANGLE_RAD);
+        arm.setBooleanDrive(true);
+    }
+    @Override
+    public void execute() {
+    arm.driveArmMax(-1);
+    arm.setSoftLimit( (float) SOFT_LIMIT_LOCATION_IN_RADIANS); //TODO: Find out what limit actually is defined as
+    }
+    @Override
+    public void end(boolean interrupted) {
+        arm.stopArm(); //OR maintain small voltage to keep arm in place?
+        arm.resetSoftLimit();
+        arm.setBooleanDrive(false);
+    }
+    @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
+    }
+}

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

@@ -56,14 +56,8 @@ public void execute() {
       intake.setRPMOutake(MANUAL_RPM_MAX * joystick.getAsDouble());
     } 
     //manipulatorController.setRumble(RumbleType.kBothRumble, 0.5);
-    if(!hasIntaked && intake.intakeDetectsNote()) {
-      manipulatorController.setRumble(RumbleType.kBothRumble, 0.5);
-      hasIntaked = true;
-      timer.start();
-    }
-    if(hasIntaked && timer.get() > 1) {
-      manipulatorController.setRumble(RumbleType.kBothRumble, 0);
-      timer.stop();
+    if(intake.intakeDetectsNote()) {
+      manipulatorController.setRumble(RumbleType.kBothRumble, 0.4);
     }
     /*
     intake.setRPMIntake(MANUAL_RPM_MAX * joystick.getAsDouble());
@@ -74,7 +68,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-
+    manipulatorController.setRumble(RumbleType.kBothRumble, 0); //Reasoning: I WANNA TEST WHAT HAPPENS WHEN ANOTHER CMD IS USED OVER DEFAULT
   }
 
   // Returns true when the command should end.

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

@@ -30,7 +30,7 @@ public moveClimber(Arm arm) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    arm.callDrive = false;//turn normal arm periodic off
+    arm.setBooleanDrive(false);//turn normal arm periodic off
     //arm.setArmTarget(goal);
     SmartDashboard.putNumber("climber volts", 0);
   }
@@ -56,7 +56,7 @@ public void execute() {
   public void end(boolean interrupted) {
     arm.driveMotor(Volt.of(0));
     arm.setArmTarget(arm.getArmPos());
-    arm.callDrive = true;
+    arm.setBooleanDrive(false);
   }
 
   // Returns true when the command should end.

src/main/java/org/carlmontrobotics/subsystems/Arm.java

@@ -10,6 +10,7 @@
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.CANSparkBase;
 import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkBase.SoftLimitDirection;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkAbsoluteEncoder;
@@ -55,8 +56,7 @@
 
 // Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2
 public class Arm extends SubsystemBase {
-    public boolean armClimbing = false;
-    public boolean callDrive = true;
+    private boolean callDrive = true;
     private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_MASTER,
             MotorConfig.NEO);
     private final CANSparkMax armMotorFollower/* right */ = MotorControllerFactory
@@ -150,6 +150,9 @@ public Arm() {
         lastArmPos = getArmPos();
         lastMeasuredTime = Timer.getFPGATimestamp();
     }
+    public void setBooleanDrive(boolean climb) {
+        callDrive = climb;
+    }
 
     @Override
     public void periodic() {
@@ -207,7 +210,7 @@ public void periodic() {
         isArmEncoderConnected = currTime - lastMeasuredTime < DISCONNECTED_ENCODER_TIMEOUT_SEC;
 
         if (isArmEncoderConnected) {
-            if (callDrive || true) {
+            if (callDrive) {
                 driveArm();
             }
         } else {
@@ -219,7 +222,7 @@ public void periodic() {
         autoCancelArmCommand();
 
     }
-
+    
     public void autoCancelArmCommand() {
         if (!(getDefaultCommand() instanceof TeleopArm) || DriverStation.isAutonomous())
             return;
@@ -260,6 +263,14 @@ public void stopArm() {
         armMotorMaster.set(0);
 
     }
+    public void driveArmMax(int direction) {
+       drivearm(6 * direction); //STARTING WITH SLOWER SPEED FOR TESTING
+    }
+    public void setLimitsForClimbOn() {
+        armPIDMaster.setOutputRange(-12, 12);
+        armMotorMaster.setOpenLoopRampRate(5);
+        armMotorMaster.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    }
     /**
      *
      * @param targetPos in radians
@@ -321,7 +332,14 @@ 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);
+        armMotorMaster.enableSoftLimit(SoftLimitDirection.kReverse, false);
+    }
     public TrapezoidProfile.State getCurrentArmState() {
         return new TrapezoidProfile.State(getArmPos(), getArmVel());
     }
@@ -363,6 +381,8 @@ public void initSendable(SendableBuilder builder){
         builder.addBooleanProperty("ArmPIDAtSetpoint", () -> armAtSetpoint(), null);
         builder.addDoubleProperty("Arm Goal Pos (rad)", () -> goalState.position, null);
         builder.addDoubleProperty("InternalArmVelocity", () -> armMasterEncoder.getVelocity(), null);
+        builder.addDoubleProperty("Soft limit Forward", () -> armMotorMaster.getSoftLimit(SoftLimitDirection.kForward), null);
+        builder.addDoubleProperty("Soft limit Reverse", () -> armMotorMaster.getSoftLimit(SoftLimitDirection.kReverse), null);
     }
 
     public double getMaxVelRad(){