diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 9195d8f..4927564 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -20,7 +20,7 @@ public class PhoenixTeleOPState extends CyberarmState { private int CyclingArmUpAndDown = 0; private double RobotRotation; private double RotationTarget, DeltaRotation; - private double MinimalPower = 0.25, topServoOffset = 0.05; + private double MinimalPower = 0.25, topServoOffset = -0.05; private GamepadChecker gamepad1Checker, gamepad2Checker; private int OCD; @@ -67,12 +67,11 @@ public class PhoenixTeleOPState extends CyberarmState { robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserLeft.setPosition(0.45); robot.LowRiserRight.setPosition(0.45); - robot.HighRiserLeft.setPosition(0.45 + topServoOffset); - robot.HighRiserRight.setPosition(0.45); + robot.HighRiserLeft.setPosition(0.45); + robot.HighRiserRight.setPosition(0.45 + topServoOffset); robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - topServoOffset = robot.HighRiserRight.getPosition() + 0.05; gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); OCD = 0; @@ -306,7 +305,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } @@ -317,7 +316,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); OCD = 0; - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } @@ -327,7 +326,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() < 0.85) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } @@ -345,7 +344,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() > 0.45 && robot.LowRiserLeft.getPosition() < 0.5) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } @@ -388,7 +387,7 @@ public class PhoenixTeleOPState extends CyberarmState { } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.46) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } else if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() <= 0.46) { @@ -407,7 +406,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.LowRiserLeft.getPosition() > 0.44 && robot.HighRiserLeft.getPosition() > 0.66) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high @@ -421,7 +420,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.64) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low @@ -441,7 +440,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() > 0.8) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } } // <-- top level too high @@ -455,7 +454,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.LowRiserLeft.getPosition() > 0.44 && robot.LowRiserLeft.getPosition() <= 0.46 && robot.HighRiserLeft.getPosition() < 0.79) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } // <-- high level too low @@ -468,7 +467,7 @@ public class PhoenixTeleOPState extends CyberarmState { if (robot.HighRiserLeft.getPosition() < 0.84) { if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05 + topServoOffset); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } }