From a8acf7c5be1f829cba6fb29a4bddd3b317a5dbfe Mon Sep 17 00:00:00 2001 From: Sodi Date: Tue, 20 Dec 2022 20:31:40 -0600 Subject: [PATCH] Adding run-to on arm heights and telemetry, finished the high junction --- .../TeleOp/states/PhoenixTeleOPState.java | 69 +++++++++---------- 1 file changed, 33 insertions(+), 36 deletions(-) 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 cb10411..33ee29e 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -21,7 +21,7 @@ public class PhoenixTeleOPState extends CyberarmState { private double RotationTarget, DeltaRotation; private double MinimalPower = 0.2; private GamepadChecker gamepad1Checker, gamepad2Checker; - private int OCD, OCDSwitchHigh, OCDSwitchMed, OCDSwitchLow; + private int OCD; public PhoenixTeleOPState(PhoenixBot1 robot) { this.robot = robot; @@ -80,15 +80,15 @@ public class PhoenixTeleOPState extends CyberarmState { if (engine.gamepad1.right_trigger > 0) { drivePower = engine.gamepad1.right_trigger; robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower - 0.05); robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower - 0.05); } else if (engine.gamepad1.left_trigger > 0) { drivePower = engine.gamepad1.left_trigger; robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(-drivePower + 0.05); robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); + robot.frontRightDrive.setPower(-drivePower + 0.05); } if (Math.abs(engine.gamepad1.left_stick_y) > 0.05) { @@ -394,7 +394,7 @@ public class PhoenixTeleOPState extends CyberarmState { OCD = 1; } - if (engine.gamepad2.left_bumper) { + if (engine.gamepad2.a) { OCD = 2; } @@ -408,14 +408,14 @@ public class PhoenixTeleOPState extends CyberarmState { if (OCD == 1) { if (robot.HighRiserLeft.getPosition() < 0.84) { - if (System.currentTimeMillis() - lastStepTime >= 150) { + if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); } } if (robot.LowRiserLeft.getPosition() <= 0.64) { - if (System.currentTimeMillis() - lastStepTime >= 150) { + if (System.currentTimeMillis() - lastStepTime >= 125) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); @@ -429,36 +429,33 @@ public class PhoenixTeleOPState extends CyberarmState { if (OCD == 2) { - if (robot.HighRiserLeft.getPosition() < 0.85) { - if (robot.LowRiserLeft.getPosition() > 0.52) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } else { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } + if (robot.LowRiserLeft.getPosition() >= 0.5) { + if (System.currentTimeMillis() - lastStepTime >= 125) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } - if (robot.HighRiserRight.getPosition() > 0.87) { - if (robot.LowRiserLeft.getPosition() > 0.52) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } else { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.HighRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } + } else if (robot.LowRiserLeft.getPosition() <= 0.5 && robot.HighRiserLeft.getPosition() > 0.5) { + if (System.currentTimeMillis() - lastStepTime >= 125) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); } + } else if (robot.LowRiserLeft.getPosition() <= 0.5 && robot.HighRiserLeft.getPosition() <= 0.5) { + OCD = 0; + } + } + + if (OCD == 3) { + if (robot.LowRiserLeft.getPosition() > 0.5 && robot.HighRiserLeft.getPosition() < 0.75) { + if (System.currentTimeMillis() - lastStepTime >= 125) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } else if (robot.HighRiserLeft.getPosition() < 0.75) { + + } }