From 714000a7df53229dc094810b17e6bceb90cbd240 Mon Sep 17 00:00:00 2001 From: Sodi Date: Tue, 13 Dec 2022 20:30:43 -0600 Subject: [PATCH] Adding run-to on arm heights and telemetry --- .../TeleOp/states/PhoenixBot1.java | 2 + .../TeleOp/states/PhoenixTeleOPState.java | 150 +++++------------- 2 files changed, 38 insertions(+), 114 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index bbdf191..5588bd0 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -127,6 +127,8 @@ public class PhoenixBot1 { // Dead Wheel encoder for driving + OdometerEncoder = engine.hardwareMap.dcMotor.get("odometer encoder"); + OdometerEncoder.setDirection(DcMotorSimple.Direction.FORWARD); OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); 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 707783a..69bc73d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -1,6 +1,7 @@ package org.timecrafters.TeleOp.states; import android.annotation.SuppressLint; +import android.widget.Switch; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.Gamepad; @@ -20,6 +21,7 @@ public class PhoenixTeleOPState extends CyberarmState { private double RotationTarget, DeltaRotation; private double MinimalPower = 0.2; private GamepadChecker gamepad1Checker, gamepad2Checker; + private int OCD; public PhoenixTeleOPState(PhoenixBot1 robot) { this.robot = robot; @@ -51,6 +53,7 @@ public class PhoenixTeleOPState extends CyberarmState { engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("OCD", OCD); } @Override @@ -67,6 +70,7 @@ public class PhoenixTeleOPState extends CyberarmState { gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); + OCD = 0; } @SuppressLint("SuspiciousIndentation") @@ -386,127 +390,45 @@ public class PhoenixTeleOPState extends CyberarmState { // // } - if (engine.gamepad2.left_bumper) { - if (robot.HighRiserLeft.getPosition() < 0.73) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } - } - if (robot.HighRiserLeft.getPosition() > 0.77 && robot.LowRiserLeft.getPosition() < 0.47) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); - } - } - if (robot.HighRiserLeft.getPosition() > 0.77 && robot.LowRiserLeft.getPosition() > 0.47) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - 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 (!engine.gamepad2.left_bumper) { -// if (robot.HighRiserLeft.getPosition() < 0.73) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(0.73); -// robot.HighRiserRight.setPosition(0.73); -// } -// } -// } + if (engine.gamepad2.right_bumper) { + OCD = 1; } - if (engine.gamepad2.right_bumper) { - if (robot.HighRiserLeft.getPosition() < 0.85) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } - } - - if (robot.LowRiserLeft.getPosition() < 0.6 && robot.HighRiserLeft.getPosition() >= 0.85) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } - if (robot.LowRiserLeft.getPosition() > 0.8) { - if (robot.HighRiserLeft.getPosition() > 0.87 && robot.LowRiserLeft.getPosition() < 0.7) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); - } - } - } -// if (!engine.gamepad2.right_bumper) { -// if (robot.HighRiserLeft.getPosition() < 0.85) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); -// } -// } -// -// if (robot.LowRiserLeft.getPosition() < 0.6 && robot.HighRiserLeft.getPosition() >= 0.85) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); -// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); -// } -// } -// if (robot.HighRiserLeft.getPosition() > 0.87) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); -// } -// } -// } + if (engine.gamepad2.left_bumper) { + OCD = 2; } if (engine.gamepad2.x) { - if (robot.HighRiserLeft.getPosition() < 0.85) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } - } - - if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } - } -// if (!engine.gamepad2.x) { -// if (robot.HighRiserLeft.getPosition() < 0.85) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); -// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); -// } -// } -// -// if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) { -// if (System.currentTimeMillis() - lastStepTime >= 150) { -// lastStepTime = System.currentTimeMillis(); -// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); -// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); -// } -// } -// } + OCD = 3; } + if (engine.gamepad2.b) { + OCD = 4; + } + + if (OCD == 1) { + switch (OCD) { + case 1: + if (robot.HighRiserLeft.getPosition() > 0.5 && robot.LowRiserLeft.getPosition() < 0.5) { + if (System.currentTimeMillis() - lastStepTime >= 150) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); + } + } + + if (robot.LowRiserLeft.getPosition() > 0.5) { + if (System.currentTimeMillis() - lastStepTime >= 150) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } + case 2: + + } + + } gamepad1Checker.update(); gamepad2Checker.update();