From 938c18a22ebd3bbd1ed4b3c2cbbe7a2f3c63816a Mon Sep 17 00:00:00 2001 From: Sodi Date: Tue, 17 Jan 2023 19:10:26 -0600 Subject: [PATCH] Adding a rewritten teleop --- .../TeleOp/states/PhoenixTeleOPv2.java | 22 ++ .../TeleOp/states/TeleOPArmDriver.java | 168 -------------- .../TeleOp/states/TeleOPTankDriver.java | 206 ------------------ 3 files changed, 22 insertions(+), 374 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java index 31058c9..606e81a 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java @@ -3,6 +3,7 @@ package org.timecrafters.TeleOp.states; import org.cyberarm.engine.V2.CyberarmState; public class PhoenixTeleOPv2 extends CyberarmState { + private double drivePower = 1; PhoenixBot1 robot; public PhoenixTeleOPv2(PhoenixBot1 robot) { this.robot = robot; @@ -12,11 +13,32 @@ public class PhoenixTeleOPv2 extends CyberarmState { public void start() { addParallelState(new TeleOPArmDriver(robot)); addParallelState(new TeleOPTankDriver(robot)); + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! + double x = engine.gamepad1.left_stick_x; + double rx = engine.gamepad1.right_stick_x; + + double backLeftPower = (y - x + rx); + double backRightPower = (y + x - rx); + double frontLeftPower = (y + x + rx); + double frontRightPower = (y - x - rx); + + robot.frontLeftDrive.setPower(frontLeftPower * drivePower); + robot.backLeftDrive.setPower(backLeftPower * drivePower); + robot.frontRightDrive.setPower(frontRightPower * drivePower); + robot.backRightDrive.setPower(backRightPower * drivePower); + + } + + @Override + public void init() { } @Override public void exec() { + if (engine.gamepad1.left_stick_x > 0.1) { + + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java index 958d925..ab66ce5 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -41,174 +41,6 @@ public class TeleOPArmDriver extends CyberarmState { @Override public void exec() { - if (engine.gamepad2.dpad_left) { - robot.collectorLeft.setPower(-1); - robot.collectorRight.setPower(-1); - } else if (engine.gamepad2.dpad_right) { - robot.collectorLeft.setPower(1); - robot.collectorRight.setPower(1); - } else { - robot.collectorLeft.setPower(0); - robot.collectorRight.setPower(0); - } - - if (engine.gamepad2.dpad_up) { - if (robot.HighRiserLeft.getPosition() < 1.0) { - 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.dpad_down) { - if (robot.HighRiserLeft.getPosition() > 0.45) { - 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.y) { - if (robot.HighRiserLeft.getPosition() < 0.9) { - 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); - } - } - }//end of y - - if (engine.gamepad2.a) { - 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); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); - } - } - - if (robot.LowRiserLeft.getPosition() > 0.45) { - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } - } - }//end of a - - if (engine.gamepad2.back) { - robot.backLeftDrive.setPower(1); - robot.backRightDrive.setPower(1); - robot.frontLeftDrive.setPower(1); - robot.frontRightDrive.setPower(1); - if (System.currentTimeMillis() - lastStepTime >= 1500) { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - } - if (System.currentTimeMillis() - lastStepTime >= 150) { - if (robot.HighRiserLeft.getPosition() < 1) { - 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 (System.currentTimeMillis() - lastStepTime >= 150) { - if (robot.LowRiserLeft.getPosition() < 1 && robot.HighRiserLeft.getPosition() == 1) { - 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 (System.currentTimeMillis() >= 250) { - robot.backLeftDrive.setPower(1); - robot.backRightDrive.setPower(1); - robot.frontLeftDrive.setPower(1); - robot.frontRightDrive.setPower(1); - if (System.currentTimeMillis() - lastStepTime >= 250) { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - } - } - - } - - - - if (engine.gamepad2.start) { - - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - - switch (CyclingArmUpAndDown) { - - // upper arm up - case 0: - if (robot.HighRiserLeft.getPosition() < 1) { - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } else { - CyclingArmUpAndDown = CyclingArmUpAndDown + 1; - } - break; - - // lower arm up - case 1: - if (robot.LowRiserLeft.getPosition() < 1) { - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } else { - CyclingArmUpAndDown = CyclingArmUpAndDown + 1; - } - break; - - // lower arm down - case 2: - if (robot.LowRiserLeft.getPosition() >= 0.44) { - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } else { - CyclingArmUpAndDown = CyclingArmUpAndDown + 1; - } - break; - - // upper arm down - case 3: - if (robot.HighRiserLeft.getPosition() >= 0.45) { - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); - } else { - CyclingArmUpAndDown = 0; - } - break; - - default: - break; - - } // end of switch - }// end of time if statement - }// end of start button press - - gamepad2Checker.update(); } } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java index 39b5d0f..ee017f1 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java @@ -33,213 +33,7 @@ public class TeleOPTankDriver extends CyberarmState { @Override public void exec() { - if (engine.gamepad1.right_trigger > 0) { - drivePower = engine.gamepad1.right_trigger; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } - if (engine.gamepad1.left_trigger > 0) { - drivePower = engine.gamepad1.left_trigger; - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - - if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { - drivePower = engine.gamepad1.left_stick_y; - robot.backRightDrive.setPower(drivePower * 0.95); - robot.frontRightDrive.setPower(drivePower * 0.95); - } - - if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { - drivePower = engine.gamepad1.right_stick_y; - robot.backLeftDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - } - - if (engine.gamepad1.right_trigger < 0.1 && - engine.gamepad1.left_trigger < 0.1 && - !engine.gamepad1.y && - !engine.gamepad1.x && - !engine.gamepad1.a && - !engine.gamepad1.b && - !engine.gamepad1.dpad_left && - !engine.gamepad1.dpad_right && - Math.abs (engine.gamepad1.left_stick_y) < 0.1 && - Math.abs(engine.gamepad1.right_stick_y) < 0.1) { - drivePower = 0; - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - - if (engine.gamepad1.a) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 180; - CalculateDeltaRotation(); - if (RobotRotation < 0 && RobotRotation > -179) { - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - else if (RobotRotation > 0) { - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - else if (RobotRotation <= -179 || RobotRotation >= 179) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - if (engine.gamepad1.y) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 0; - CalculateDeltaRotation(); - if (RobotRotation < -1) { - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation > 1) { - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation > -1 && RobotRotation < 1) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - if (engine.gamepad1.dpad_left) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = -45; - CalculateDeltaRotation(); - if (RobotRotation > -45 && RobotRotation <= 135) {//CCW - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -45 || RobotRotation > 136) {//CW - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -44 && RobotRotation > -46) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - if (engine.gamepad1.x) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = -90; - CalculateDeltaRotation(); - if (RobotRotation < 90 && RobotRotation < -89) {//CCW - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower * 0.95); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower * 0.95); - } - if (RobotRotation > 90 || RobotRotation < -91) {//CW - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation > -91 && RobotRotation < -89) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - if (engine.gamepad1.dpad_right) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 45; - CalculateDeltaRotation(); - if (RobotRotation > -135 && RobotRotation < 44) {//CCW - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -135 || RobotRotation < 46) {//CW - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < 46 && RobotRotation > 44) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - if (engine.gamepad1.b) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 90; - CalculateDeltaRotation(); - if (RobotRotation > -90 && RobotRotation < 89) {//CCW - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -90 || RobotRotation > 91) {//CW - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < 91 && RobotRotation > 89) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - gamepad1Checker.update(); } public void CalculateDeltaRotation() { if (RotationTarget >= 0 && RobotRotation >= 0) {