fixed formatting and added number to pos tolerance(NOT FINALIZED NUMBER)

DeepBlueRobotics · Mar 5, 2024 · 071ebe6 · 071ebe6
1 parent 1919ce2
commit 071ebe6
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 8 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -74,11 +74,13 @@ public static final class Arm {
 				MAX_FF_VEL_RAD_P_S, MAX_FF_ACCEL_RAD_P_S);
 		// other0;
 
-		//public static final double MARGIN_OF_ERROR = Math.PI / 18;
+		// public static final double MARGIN_OF_ERROR = Math.PI / 18;
 
 		// Boundaries
 		public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; // placeholder
-		public static final double POS_TOLERANCE_RAD = 0; // placeholder
+		public static final double POS_TOLERANCE_RAD = Units.degreesToRadians(195); // placeholder //Whether or not this is the actual account
+															// idk TODO: test on actual encoder without a conversion
+															// factor
 		public static final double VEL_TOLERANCE_RAD_P_SEC = 0; // placeholder
 		public static final double UPPER_ANGLE_LIMIT_RAD = Units.degreesToRadians(70);
 		public static final double LOWER_ANGLE_LIMIT_RAD = Units.degreesToRadians(0);

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -80,7 +80,7 @@ private void setBindingsManipulator() {
     // 3 setpositions of arm on letterpad
     // Up is Speaker, down is ground, right is Amp
     // right joystick used for manual arm control
-    //COMMENT THESE OUT DURING SYSID TESTING
+    // COMMENT THESE OUT DURING SYSID TESTING
     // Speaker Buttons
     new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_POD_BUTTON)
         .onTrue(new InstantCommand(() -> {
@@ -112,7 +112,7 @@ private void setBindingsManipulator() {
         .onTrue(new InstantCommand(() -> {
           arm.setArmTarget(CLIMBER_DOWN_ANGLE_RAD);
         }));
-    //----------------------------------------------------------------------
+    // ----------------------------------------------------------------------
   }
 
   public Command getAutonomousCommand() {

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -166,10 +166,9 @@ private void driveArm() {
             armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);
         }
         armPIDMaster.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts);
-        if (armAtSetpoint() || getArmPos() > setPoint.position){
-            armPIDMaster.setIZone(Double.POSITIVE_INFINITY);//turns off pid once it reaches the setpoint
-        }
-        else {
+        if (armAtSetpoint() || getArmPos() > setPoint.position) {
+            armPIDMaster.setIZone(Double.POSITIVE_INFINITY);// turns off pid once it reaches the setpoint
+        } else {
             armPIDMaster.setIZone(IZONE);
         }