From fba5283407b4d74acbb23e7c6cad6d6bdc872b17 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 7 Nov 2022 16:35:59 -0600 Subject: [PATCH] RightSideAutonomous and leftsideAutonomous work added --- .../Engines/LeftSideAutonomousEngine.java | 96 ++++++++++++++++ .../Engines/RightSideAutonomousEngine.java | 94 +++++++++++++++ .../Engines/TestAutonomousEngine.java | 89 --------------- .../States/CollectorDistanceState.java | 64 +++++------ .../States/DriverParkPlaceState.java | 108 ++++++++++++++++++ .../Autonomous/States/PathDecision.java | 12 +- .../Autonomous/States/RotationState.java | 62 +++++----- 7 files changed, 362 insertions(+), 163 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java new file mode 100644 index 0000000..ebee709 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -0,0 +1,96 @@ +package org.timecrafters.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.Autonomous.States.BottomArm; +import org.timecrafters.Autonomous.States.CollectorDistanceState; +import org.timecrafters.Autonomous.States.CollectorState; +import org.timecrafters.Autonomous.States.ConeIdentification; +import org.timecrafters.Autonomous.States.DriverParkPlaceState; +import org.timecrafters.Autonomous.States.DriverState; +import org.timecrafters.Autonomous.States.PathDecision; +import org.timecrafters.Autonomous.States.RotationState; +import org.timecrafters.Autonomous.States.TopArm; +import org.timecrafters.testing.states.PrototypeBot1; + +@Autonomous (name = "Left Side") + +public class LeftSideAutonomousEngine extends CyberarmEngine { + PrototypeBot1 robot; + + @Override + public void setup() { + robot = new PrototypeBot1(this); + addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0")); + //drive to high pole + addState(new DriverState(robot, "LeftSideAutonomous", "01-0")); + //turn towards high pole + addState(new RotationState(robot, "LeftSideAutonomous", "02-0")); + //lift the upper arm + addState(new TopArm(robot, "LeftSideAutonomous", "03-0")); + //lift the lower arm + addState(new BottomArm(robot, "LeftSideAutonomous", "04-0")); + //drive forward + addState(new DriverState(robot, "LeftSideAutonomous", "05-0")); + //lower the bottom arm to get closer + addState(new TopArm(robot, "LeftSideAutonomous", "06-0")); + //make collector release the cone + addState(new CollectorState(robot, "LeftSideAutonomous", "07-0")); + //lift the lower arm to clear the pole + addState(new TopArm(robot, "LeftSideAutonomous", "08-0")); + //Drive Backwards + addState(new DriverState(robot, "LeftSideAutonomous", "09-0")); + // Rotate to either set up for cones to grab or park somewhere + addState(new RotationState(robot, "LeftSideAutonomous", "12-0")); + // lower the bottom arm so we dont fall over + addState(new BottomArm(robot, "LeftSideAutonomous", "10-0")); + // lower the upper arm so we dont fall over + addState(new TopArm(robot, "LeftSideAutonomous", "11-0"));; + //adjust arm height to cone. + addState(new TopArm(robot, "LeftSideAutonomous", "13-0")); + //drive towards stack of cones while collecting + addState(new CollectorDistanceState(robot, "LeftSideAutonomous", "14-0")); + //drive slightly back + addState(new DriverState(robot, "LeftSideAutonomous", "15-0")); + // face to -90 + addState(new RotationState(robot, "LeftSideAutonomous", "15-1")); + //lift arm up + addState(new TopArm(robot, "LeftSideAutonomous", "16-0")); + // drive backwards too position + addState(new DriverState(robot, "LeftSideAutonomous", "17-0")); + // rotate + addState(new RotationState(robot, "LeftSideAutonomous", "18-0")); + + addState(new DriverState(robot, "LeftSideAutonomous", "21-0")); + // bring arm down. + addState(new TopArm(robot, "LeftSideAutonomous", "22-0")); + // get rid of cone + addState(new CollectorState(robot, "LeftSideAutonomous", "23-0")); + // lift arm up to clear + addState(new TopArm(robot, "LeftSideAutonomous", "24-0")); + // rotate towards stack of cones + addState(new RotationState(robot, "LeftSideAutonomous", "28-0")); + // Choose Parking Spot + addState(new PathDecision(robot, "LeftSideAutonomous", "29-0")); + // case 1 drive forward + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-1")); + // case 2 drive forward + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-2")); + // case 3 drive forward + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-3")); + // zero out + addState(new RotationState(robot, "LeftSideAutonomous", "30-0")); + // Top Arm Down + addState(new TopArm(robot, "LeftSideAutonomous", "28-1")); + //Drive Backwards + addState(new DriverState(robot, "LeftSideAutonomous", "30-1")); + } + + @Override + public void loop() { + super.loop(); + + telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java new file mode 100644 index 0000000..6b0339c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -0,0 +1,94 @@ +package org.timecrafters.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.Autonomous.States.CollectorDistanceState; +import org.timecrafters.Autonomous.States.CollectorState; +import org.timecrafters.Autonomous.States.ConeIdentification; +import org.timecrafters.Autonomous.States.DriverParkPlaceState; +import org.timecrafters.Autonomous.States.DriverState; +import org.timecrafters.Autonomous.States.BottomArm; +import org.timecrafters.Autonomous.States.PathDecision; +import org.timecrafters.Autonomous.States.RotationState; +import org.timecrafters.Autonomous.States.TopArm; +import org.timecrafters.testing.states.PrototypeBot1; + +@Autonomous (name = "Right Side") + +public class RightSideAutonomousEngine extends CyberarmEngine { + PrototypeBot1 robot; + + @Override + public void setup() { + robot = new PrototypeBot1(this); + addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0")); + //drive to high pole + addState(new DriverState(robot, "RightSideAutonomous", "01-0")); + //turn towards high pole + addState(new RotationState(robot, "RightSideAutonomous", "02-0")); + //lift the upper arm + addState(new TopArm(robot, "RightSideAutonomous", "03-0")); + //lift the lower arm + addState(new BottomArm(robot, "RightSideAutonomous", "04-0")); + //drive forward + addState(new DriverState(robot, "RightSideAutonomous", "05-0")); + //lower the bottom arm to get closer + addState(new TopArm(robot, "RightSideAutonomous", "06-0")); + //make collector release the cone + addState(new CollectorState(robot, "RightSideAutonomous", "07-0")); + //lift the lower arm to clear the pole + addState(new TopArm(robot, "RightSideAutonomous", "08-0")); + //Drive Backwards + addState(new DriverState(robot, "RightSideAutonomous", "09-0")); + // lower the bottom arm so we dont fall over + addState(new BottomArm(robot, "RightSideAutonomous", "10-0")); + // lower the upper arm so we dont fall over + addState(new TopArm(robot, "RightSideAutonomous", "11-0"));; + // Rotate to either set up for cones to grab or park somewhere + addState(new RotationState(robot, "RightSideAutonomous", "12-0")); + //adjust arm height to cone. + addState(new TopArm(robot, "RightSideAutonomous", "13-0")); + //drive towards stack of cones while collecting + addState(new CollectorDistanceState(robot, "RightSideAutonomous", "14-0")); + //drive slightly back + addState(new DriverState(robot, "RightSideAutonomous", "15-0")); + // face to -90 + addState(new RotationState(robot, "RightSideAutonomous", "15-1")); + //lift arm up + addState(new TopArm(robot, "RightSideAutonomous", "16-0")); + // drive backwards too position + addState(new DriverState(robot, "RightSideAutonomous", "17-0")); + // rotate + addState(new RotationState(robot, "RightSideAutonomous", "18-0")); + + addState(new DriverState(robot, "RightSideAutonomous", "21-0")); + // bring arm down. + addState(new TopArm(robot, "RightSideAutonomous", "22-0")); + // get rid of cone + addState(new CollectorState(robot, "RightSideAutonomous", "23-0")); + // lift arm up to clear + addState(new TopArm(robot, "RightSideAutonomous", "24-0")); + // rotate towards stack of cones + addState(new RotationState(robot, "RightSideAutonomous", "28-0")); + // Choose Parking Spot + addState(new PathDecision(robot, "RightSideAutonomous", "29-0")); + // case 1 drive forward + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-1")); + // case 2 drive forward + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-2")); + // case 3 drive forward + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-3")); + // zero out + addState(new RotationState(robot, "RightSideAutonomous", "30-0")); + // Top Arm Down + addState(new TopArm(robot, "RightSideAutonomous", "28-1")); + } + + @Override + public void loop() { + super.loop(); + + telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java deleted file mode 100644 index b64a4f9..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ /dev/null @@ -1,89 +0,0 @@ -package org.timecrafters.Autonomous.Engines; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.Autonomous.States.CollectorDistanceState; -import org.timecrafters.Autonomous.States.CollectorState; -import org.timecrafters.Autonomous.States.ConeIdentification; -import org.timecrafters.Autonomous.States.DriverState; -import org.timecrafters.Autonomous.States.BottomArm; -import org.timecrafters.Autonomous.States.RotationState; -import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.testing.states.PrototypeBot1; - -@Autonomous (name = "Autonomous Test") - -public class TestAutonomousEngine extends CyberarmEngine { - PrototypeBot1 robot; - - @Override - public void setup() { - robot = new PrototypeBot1(this); - addState(new ConeIdentification(robot, "TestAutonomous", "00-0")); - //drive to high pole - addState(new DriverState(robot, "TestAutonomous", "01-0")); - //turn towards high pole - addState(new RotationState(robot, "TestAutonomous", "02-0")); - //lift the upper arm - addState(new TopArm(robot, "TestAutonomous", "03-0")); - //lift the lower arm - addState(new BottomArm(robot, "TestAutonomous", "04-0")); - //drive forward - addState(new DriverState(robot, "TestAutonomous", "05-0")); - //lower the bottom arm to get closer - addState(new TopArm(robot, "TestAutonomous", "06-0")); - //make collector release the cone - addState(new CollectorState(robot, "TestAutonomous", "07-0")); - //lift the lower arm to clear the pole - addState(new TopArm(robot, "TestAutonomous", "08-0")); - //Drive Backwards - addState(new DriverState(robot, "TestAutonomous", "09-0")); - // lower the bottom arm so we dont fall over - addState(new BottomArm(robot, "TestAutonomous", "10-0")); - // lower the upper arm so we dont fall over - addState(new TopArm(robot, "TestAutonomous", "11-0"));; - // Rotate to either set up for cones to grab or park somewhere - addState(new RotationState(robot, "TestAutonomous", "12-0")); - //adjust arm height to cone. - addState(new TopArm(robot, "TestAutonomous", "13-0")); - //drive towards stack of cones while collecting - addState(new CollectorDistanceState(robot, "TestAutonomous", "14-0")); - //drive slightly back - addState(new DriverState(robot, "TestAutonomous", "15-0")); - //lift arm up - addState(new TopArm(robot, "TestAutonomous", "16-0")); - // drive backwards too position - addState(new DriverState(robot, "TestAutonomous", "17-0")); - // rotate - addState(new RotationState(robot, "TestAutonomous", "18-0")); - //lift the upper arm - addState(new TopArm(robot, "TestAutonomous", "19-0")); - //lift the lower arm - addState(new BottomArm(robot, "TestAutonomous", "20-0")); - //drive forward to allign - addState(new DriverState(robot, "TestAutonomous", "21-0")); - // bring arm down. - addState(new TopArm(robot, "TestAutonomous", "22-0")); - // get rid of cone - addState(new CollectorState(robot, "TestAutonomous", "23-0")); - // lift arm up to clear - addState(new TopArm(robot, "TestAutonomous", "24-0")); - // drive back - addState(new DriverState(robot, "TestAutonomous", "25-0")); - // bring bottom arm down - addState(new BottomArm(robot, "TestAutonomous", "26-0")); - // bring top arm down - addState(new TopArm(robot, "TestAutonomous", "27-0")); - // rotate towards stack of cones - addState(new RotationState(robot, "TestAutonomous", "28-0")); - } - - @Override - public void loop() { - super.loop(); - - telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index fc7e32a..b6982e4 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -102,45 +102,46 @@ public class CollectorDistanceState extends CyberarmState { // and im close too my target. LastDistanceRead = currentDistance; debugRunTime = runTime(); - debugStatus = "RUNNING"; + debugStatus = "Driving Towards Cone"; } else { // I have stopped - debugStatus = "STOPPED"; + debugStatus = "Nothing Collected"; + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); setHasFinished(true); return; } } - if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { - double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); - if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){ - // ramping up - drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15; - } - else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){ - // ramping down - drivePower = ((delta / RampDownDistance) + 0.15); - } else { - // middle ground - drivePower = targetDrivePower; - } + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) { + // ramping up + drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15; + } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) { + // ramping down + drivePower = ((delta / RampDownDistance) + 0.15); + } else { + // middle ground + drivePower = targetDrivePower; + } - if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ - // This is limiting drive power to the targeted drive power - drivePower = targetDrivePower; - } + if (Math.abs(drivePower) > Math.abs(targetDrivePower)) { + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } - if (targetDrivePower < 0 && drivePower != targetDrivePower) { - drivePower = drivePower * -1; - } + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } - if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - } + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) { + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } } else { robot.frontRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); @@ -150,10 +151,9 @@ public class CollectorDistanceState extends CyberarmState { robot.collectorLeft.setPower(0); setHasFinished(true); - } + } + + } } - - -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java new file mode 100644 index 0000000..5b918a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -0,0 +1,108 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class DriverParkPlaceState extends CyberarmState { + private final boolean stateDisabled; + PrototypeBot1 robot; + private int RampUpDistance; + private int RampDownDistance; + private String intendedPlacement; + + public DriverParkPlaceState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); + this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); + this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } + private double drivePower, targetDrivePower; + private int traveledDistance; + + @Override + public void start() { + robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + @Override + public void exec() { + if (stateDisabled) { + setHasFinished(true); + return; + } + String placement = engine.blackboard.get("parkPlace"); + if (placement != null) { + if (!placement.equals(intendedPlacement)){ + setHasFinished(true); + } + if (placement.equals(intendedPlacement)) { + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) { + // ramping up + drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; + } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) { + // ramping down + drivePower = ((delta / RampDownDistance) + 0.25); + } else { + // middle ground + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)) { + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } + + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) { + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } else { + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + setHasFinished(true); + } // else ending + } // placement equals if statement +// setHasFinished(true); + } // end of placement doesn't equal null + + } // end of exec + + @Override + public void telemetry() { + engine.telemetry.addData("Position", intendedPlacement); + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addData("traveledDistance", traveledDistance); + engine.telemetry.addData("RampUpDistance", RampUpDistance); + engine.telemetry.addData("RampDownDistance", RampDownDistance); + + } // end of telemetry +} // end of class diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java index 97c4ae4..672b240 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -17,17 +17,7 @@ public class PathDecision extends CyberarmState { @Override public void exec() { String placement = engine.blackboard.get("parkPlace"); - - if (placement != null) { - if (placement.equals("2")) { - engine.insertState(this, new DriverState(robot, groupName, "29-1")); - } else if (placement.equals("3")) { - engine.insertState(this, new DriverState(robot, groupName, "29-2")); - } - } - else { - engine.insertState(this, new DriverState(robot, groupName, "29-0")); - } + setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index 25626bd..defdcc6 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -8,8 +8,9 @@ public class RotationState extends CyberarmState { PrototypeBot1 robot; public RotationState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; - this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); + this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value(); + drivePowerVariable = drivePower; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -18,46 +19,41 @@ public class RotationState extends CyberarmState { private double drivePower; private float targetRotation; float RobotRotation; - private double RotationTarget, DeltaRotation; + private double RotationTarget; private double RotationDirectionMinimum; + private String debugStatus = "?"; + private double drivePowerVariable; - public void CalculateDeltaRotation() { - if (RotationTarget >= 0 && RobotRotation >= 0) { - DeltaRotation = Math.abs(RotationTarget - RobotRotation); - } - else if (RotationTarget <= 0 && RobotRotation <= 0) { - DeltaRotation = Math.abs(RotationTarget - RobotRotation); - } - else if (RotationTarget >= 0 && RobotRotation <= 0) { - DeltaRotation = Math.abs(RotationTarget + RobotRotation); - } - else if (RotationTarget <=0 && RobotRotation >= 0) { - DeltaRotation = Math.abs(RobotRotation + RotationTarget); - } - } - @Override public void exec() { if (stateDisabled){ setHasFinished(true); return; - } + } // end of if RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = targetRotation; - CalculateDeltaRotation(); - if (drivePower < 0){ - RotationDirectionMinimum = -0.3; - } else { - RotationDirectionMinimum = 0.3; + + if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){ + drivePowerVariable = 0.3 * drivePower; + debugStatus = "Rotate Slow"; + } // end of if + else { + drivePowerVariable = drivePower * 0.75; + debugStatus = "Rotate Fast"; } - drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum; - if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) { - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); + + if (RobotRotation >= targetRotation + 1){ + drivePowerVariable = Math.abs(drivePowerVariable); + } else { + drivePowerVariable = -1 * Math.abs(drivePowerVariable); + } + + if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) { + robot.backLeftDrive.setPower(drivePowerVariable); + robot.backRightDrive.setPower(-drivePowerVariable); + robot.frontLeftDrive.setPower(drivePowerVariable); + robot.frontRightDrive.setPower(-drivePowerVariable); } else { robot.backLeftDrive.setPower(0); @@ -71,9 +67,13 @@ public class RotationState extends CyberarmState { @Override public void telemetry() { + engine.telemetry.addData("DEBUG Status", debugStatus); + + engine.telemetry.addLine(); + engine.telemetry.addData("Robot IMU Rotation", RobotRotation); engine.telemetry.addData("Robot Target Rotation", targetRotation); - engine.telemetry.addData("Drive Power", drivePower); + engine.telemetry.addData("Drive Power", drivePowerVariable); } }