From 8bed3ffefd6f6787c874eea26b6f7e34a3e223e0 Mon Sep 17 00:00:00 2001 From: Sodi Date: Tue, 24 Jan 2023 20:37:03 -0600 Subject: [PATCH] Adding semi-autonomous functions and transferring Teleop code, variable names are *NOT* inspired by the story of Spirit and Opportunity, the names are a coincidence. --- .../TeleOp/states/PhoenixTeleOPState.java | 3 +- .../TeleOp/states/TeleOPArmDriver.java | 149 +++++++++++++++++- .../TeleOp/states/TeleOPTankDriver.java | 44 ++++-- 3 files changed, 174 insertions(+), 22 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 5098fed..822d342 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -493,5 +493,4 @@ public class PhoenixTeleOPState extends CyberarmState { // Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5; // return Distance; - } - + } \ No newline at end of file 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 4ea2779..66683f0 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -13,6 +13,15 @@ public class TeleOPArmDriver extends CyberarmState { private GamepadChecker gamepad2Checker; private int Opportunity, Endeavour; private double drivePower; + private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05; + private double servoCollectLow = 0.40; //Low servos, A button + private double servoCollectHigh = 0.40; //High servos, A button + private double servoLowLow = 0.5; //Low servos, X button + private double servoLowHigh = 0.75; //High servos, X button + private double servoMedLow = 0.5; //Low servos, B button + private double servoMedHigh = 0.9; //High servos, B button + private double servoHighLow = 0.8; //Low servos, Y button + private double servoHighHigh = 0.9; //High servos, Y button public TeleOPArmDriver(PhoenixBot1 robot) { this.robot = robot; @@ -38,23 +47,147 @@ public class TeleOPArmDriver extends CyberarmState { robot.HighRiserLeft.setPosition(0.45); robot.HighRiserRight.setPosition(0.45); Opportunity = 0; - Endeavour = 10; + Endeavour = 0; gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); } - @Override - public void exec() { - if (robot.collectorDistance.getDistance(DistanceUnit.MM) < 275 && robot.collectorDistance.getDistance(DistanceUnit.MM) > 100) { - Endeavour = 0; - } +@Override +public void exec() { - if (robot.collectorDistance.getDistance(DistanceUnit.MM) < 550 && robot.collectorDistance.getDistance(DistanceUnit.MM) > 275) { + if (engine.gamepad2.y) { + Endeavour = 4; + } + if (engine.gamepad2.b) { + Endeavour = 3; + } + if (engine.gamepad2.x) { + Endeavour = 2; + } + if (engine.gamepad2.a) { Endeavour = 1; } + if (Endeavour == 4) { + if (robot.HighRiserLeft.getPosition() < servoHighHigh - 0.01)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() < servoHighLow - 0.01)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); + } + } + if (robot.HighRiserLeft.getPosition() >= servoHighHigh && + robot.LowRiserLeft.getPosition() >= servoHighLow) { + Endeavour = 0; + } } + if (Endeavour == 3) { + if (robot.LowRiserLeft.getPosition() > servoMedLow + 0.01)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } + if (robot.LowRiserLeft.getPosition() < servoMedLow - 0.01)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() <= servoMedLow + 0.01 && + robot.HighRiserLeft.getPosition() > servoMedHigh + 0.01)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); + } + } + if (robot.LowRiserLeft.getPosition() < servoMedLow + 0.01 && + robot.HighRiserLeft.getPosition() < servoMedHigh - 0.01)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() > servoMedLow - 0.01 && + robot.LowRiserLeft.getPosition() <= servoMedLow && + robot.HighRiserLeft.getPosition() > servoMedHigh - 0.01 && + robot.HighRiserLeft.getPosition() <= servoMedHigh) { + Endeavour = 0; + } + } - } \ No newline at end of file + if (Endeavour == 2) { + if (robot.LowRiserLeft.getPosition() > servoLowLow + 0.01)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } + if (robot.LowRiserLeft.getPosition() < servoLowLow - 0.01)/* <-- low level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && + robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 && + robot.HighRiserLeft.getPosition() > servoLowHigh)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); + } + } + if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && + robot.HighRiserLeft.getPosition() < servoLowHigh - 0.01)/* <-- high level too low*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); + } + } + if (robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 && + robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 && + robot.HighRiserLeft.getPosition() > servoLowHigh - 0.01 && + robot.HighRiserLeft.getPosition() <= servoLowHigh + 0.01) { + Endeavour = 0; + } + } + + if (Endeavour == 1) { + if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 0.01)/* <-- low level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); + } + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && + robot.HighRiserLeft.getPosition() > servoCollectHigh)/* <-- high level too high*/ { + if (System.currentTimeMillis() - lastStepTime >= 100) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); + } + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 0.01 && + robot.HighRiserLeft.getPosition() <= servoCollectHigh) { + Endeavour = 0; + } + } + + } +} \ No newline at end of file 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 d1efb8f..afe1d85 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java @@ -12,11 +12,13 @@ public class TeleOPTankDriver extends CyberarmState { private long lastStepTime = 0; private double drivePower = 0.3; private double RobotRotation; + private double currentDriveCommand; private double RotationTarget, DeltaRotation; private double MinimalPower = 0.2; private int DeltaOdometerR, Endeavour, Spirit; private boolean FreeSpirit; private GamepadChecker gamepad1Checker; + public TeleOPTankDriver(PhoenixBot1 robot) { this.robot = robot; } @@ -46,7 +48,8 @@ public class TeleOPTankDriver extends CyberarmState { } if (FreeSpirit) { - drivePower = -engine.gamepad1.left_stick_y; + getCurrentDriveCommand(); + drivePower = -currentDriveCommand; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -61,33 +64,50 @@ public class TeleOPTankDriver extends CyberarmState { robot.frontRightDrive.setPower(drivePower); } - if () - + if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { + drivePower = engine.gamepad1.right_stick_x; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } } + public void CalculateDeltaRotation() { if (RotationTarget >= 0 && RobotRotation >= 0) { DeltaRotation = Math.abs(RotationTarget - RobotRotation); - } - else if (RotationTarget <= 0 && RobotRotation <= 0) { + } else if (RotationTarget <= 0 && RobotRotation <= 0) { DeltaRotation = Math.abs(RotationTarget - RobotRotation); - } - else if (RotationTarget >= 0 && RobotRotation <= 0) { + } else if (RotationTarget >= 0 && RobotRotation <= 0) { DeltaRotation = Math.abs(RotationTarget + RobotRotation); - } - else if (RotationTarget <=0 && RobotRotation >= 0) { + } else if (RotationTarget <= 0 && RobotRotation >= 0) { DeltaRotation = Math.abs(RobotRotation + RotationTarget); } } - public double getDeltaOdometerR() { + public void getDeltaOdometerR() { Spirit = robot.OdometerEncoder.getCurrentPosition(); if (System.currentTimeMillis() - lastStepTime >= 1000) { lastStepTime = System.currentTimeMillis(); DeltaOdometerR = robot.OdometerEncoder.getCurrentPosition() - Spirit; } - return DeltaOdometerR; + } + + public void getCurrentDriveCommand() { + if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { + currentDriveCommand = engine.gamepad1.left_stick_y; + } else if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) { + currentDriveCommand = engine.gamepad1.left_stick_x; + } else if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { + currentDriveCommand = engine.gamepad1.right_stick_x; + } else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { + currentDriveCommand = engine.gamepad1.right_stick_y; + } else if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_y) > 0.1) { + currentDriveCommand = 0; + } + } @Override @@ -104,4 +124,4 @@ public class TeleOPTankDriver extends CyberarmState { } } -} +} \ No newline at end of file