diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 66adba0c..cd47e742 100644 --- 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 index e4c80ea5..0657dc65 100644 --- 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 index 5e5569a6..0862d790 100644 --- 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); }