From 495376212e22c50bf1e24201160e0bb083a55df2 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Tue, 1 Nov 2022 20:31:00 -0500 Subject: [PATCH 1/8] autnomous work added --- .../Autonomous/States/DriverState.java | 50 ++++++++++++++++++- 1 file changed, 48 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index 22ebba6..d1e4a5e 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -8,14 +8,18 @@ import org.timecrafters.testing.states.PrototypeBot1; public class DriverState extends CyberarmState { private final boolean stateDisabled; PrototypeBot1 robot; + private int RampUpDistance; + private int RampDownDistance; public DriverState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; - this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); + 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.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } - private double drivePower; + private double drivePower, targetDrivePower; private int traveledDistance; @Override @@ -38,6 +42,31 @@ public class DriverState extends CyberarmState { return; } + double delta = traveledDistance - robot.frontRightDrive.getCurrentPosition(); + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){ + drivePower = (((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; + } + else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){ + drivePower = (((delta - RampDownDistance) / RampDownDistance) + 0.25) - 1; + + if (drivePower < 0) { + + drivePower = 0.25; + + } + } else { + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ + 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); @@ -51,5 +80,22 @@ public class DriverState extends CyberarmState { setHasFinished(true); } + + } + + @Override + public void telemetry() { + 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); + } } From 863ef69690a2cc669e1e463bfcb9ece271a5d8da Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 3 Nov 2022 20:32:35 -0500 Subject: [PATCH 2/8] autnomous work added --- .../Engines/TestAutonomousEngine.java | 27 ++++++-- .../States/CollectorDistanceState.java | 68 +++++++++++++++++-- .../Autonomous/States/DriverState.java | 18 +++-- .../Autonomous/States/RotationState.java | 26 +++++++ .../testing/states/PrototypeBot1.java | 4 +- 5 files changed, 118 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index cca4e0b..50f5bce 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -47,12 +47,27 @@ public class TestAutonomousEngine extends CyberarmEngine { addState(new RotationState(robot, "TestAutonomous", "12-0")); //adjust arm height to cone. addState(new TopArm(robot, "TestAutonomous", "13-0")); - //drive towards stack of cones - addState(new CollectorDistanceState(robot, "", "")); -// DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0"); -// addState(driverState); -// //collect next cone -// driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-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")); + 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 3e8abe6..e15daac 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -9,6 +9,37 @@ import org.timecrafters.testing.states.PrototypeBot1; public class CollectorDistanceState extends CyberarmState { private final PrototypeBot1 robot; + private int traveledDistance; + private int RampUpDistance; + private int RampDownDistance; + private double drivePower; + private double targetDrivePower; + + public CollectorDistanceState(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(); + + } + + @Override + public void telemetry() { + engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM)); + 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); + + } @Override public void start() { @@ -33,10 +64,35 @@ public class CollectorDistanceState extends CyberarmState { @Override public void exec() { if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { - robot.frontRightDrive.setPower(0.15); - robot.frontLeftDrive.setPower(0.15); - robot.backRightDrive.setPower(0.15); - robot.backLeftDrive.setPower(0.15); + 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(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.frontRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); @@ -49,9 +105,7 @@ public class CollectorDistanceState extends CyberarmState { } } - public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { - this.robot = robot; -} + } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index d1e4a5e..e48552f 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -42,24 +42,22 @@ public class DriverState extends CyberarmState { return; } - double delta = traveledDistance - robot.frontRightDrive.getCurrentPosition(); + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){ - drivePower = (((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; + // ramping up + drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){ - drivePower = (((delta - RampDownDistance) / RampDownDistance) + 0.25) - 1; - - if (drivePower < 0) { - - drivePower = 0.25; - - } - } else { + // 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; } 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 4cd2124..25626bd 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -18,6 +18,24 @@ public class RotationState extends CyberarmState { private double drivePower; private float targetRotation; float RobotRotation; + private double RotationTarget, DeltaRotation; + private double RotationDirectionMinimum; + + + 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() { @@ -27,6 +45,14 @@ public class RotationState extends CyberarmState { } RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RotationTarget = targetRotation; + CalculateDeltaRotation(); + if (drivePower < 0){ + RotationDirectionMinimum = -0.3; + } else { + RotationDirectionMinimum = 0.3; + } + drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum; if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) { robot.backLeftDrive.setPower(-drivePower); robot.backRightDrive.setPower(drivePower); diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 4b7a5e1..13f1eda 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -117,8 +117,8 @@ public class PrototypeBot1 { LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserRight.setDirection(Servo.Direction.REVERSE); - LowRiserLeft.setPosition(0.45); - LowRiserRight.setPosition(0.45); + LowRiserLeft.setPosition(0.35); + LowRiserRight.setPosition(0.35); HighRiserLeft.setPosition(0.45); HighRiserRight.setPosition(0.45); From ddba73b6cb0bcf2cd7a1d993cc8b41994c93780f Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 5 Nov 2022 16:26:31 -0500 Subject: [PATCH 3/8] autnomous work added --- .../cyberarm/engine/V2/CyberarmEngine.java | 5 +- .../Engines/TestAutonomousEngine.java | 26 +++-- .../Autonomous/States/BottomArm.java | 37 ++++--- .../States/CollectorDistanceState.java | 31 ++++++ .../Autonomous/States/ConeIdentification.java | 98 +++++++++++++++++++ .../Autonomous/States/PathDecision.java | 39 ++++++++ .../testing/states/PrototypeBot1.java | 15 ++- 7 files changed, 224 insertions(+), 27 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index b96d034..6dbc79e 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -5,6 +5,8 @@ import android.util.Log; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.Gamepad; +import java.util.ArrayList; +import java.util.HashMap; import java.util.concurrent.CopyOnWriteArrayList; /** @@ -17,7 +19,8 @@ public abstract class CyberarmEngine extends OpMode { public static CyberarmEngine instance; //Array To Hold States final private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); - private int activeStateIndex = 0; + public HashMap blackboard = new HashMap<>(); + private int activeStateIndex = 0; private boolean isRunning; private final static String TAG = "PROGRAM.ENGINE"; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index 50f5bce..b64a4f9 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -6,6 +6,7 @@ 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; @@ -20,7 +21,7 @@ public class TestAutonomousEngine extends CyberarmEngine { @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 @@ -67,15 +68,22 @@ public class TestAutonomousEngine extends CyberarmEngine { 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/BottomArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java index 0865724..6c74a5c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -11,6 +11,7 @@ public class BottomArm extends CyberarmState { long time; long lastStepTime = 0; boolean up; + boolean directPosition; public BottomArm(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -18,6 +19,7 @@ public class BottomArm extends CyberarmState { this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value(); this.time = robot.configuration.variable(groupName, actionName, "time").value(); this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); + this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -35,22 +37,31 @@ public class BottomArm extends CyberarmState { return; } - if (System.currentTimeMillis() - lastStepTime >= time) { - lastStepTime = System.currentTimeMillis(); + if (directPosition) { + robot.LowRiserLeft.setPosition(LowerRiserLeftPos); + robot.LowRiserRight.setPosition(LowerRiserLeftPos); - if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { - - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); - - } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { - - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); - - } else { + if (runTime() >= time){ setHasFinished(true); } + } else { + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + + if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); + + } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); + + } else { + setHasFinished(true); + } + } } } 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 e15daac..9fc3355 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -14,6 +14,10 @@ public class CollectorDistanceState extends CyberarmState { private int RampDownDistance; private double drivePower; private double targetDrivePower; + private double lastMeasuredTime; + private double currentDistance; + private double LastDistanceRead; + public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -38,6 +42,8 @@ public class CollectorDistanceState extends CyberarmState { engine.telemetry.addData("traveledDistance", traveledDistance); engine.telemetry.addData("RampUpDistance", RampUpDistance); engine.telemetry.addData("RampDownDistance", RampDownDistance); + engine.telemetry.addData("Current Distance", currentDistance); + engine.telemetry.addData("last Distance", LastDistanceRead); } @@ -57,12 +63,37 @@ public class CollectorDistanceState extends CyberarmState { robot.collectorLeft.setPower(1); robot.collectorRight.setPower(1); + + lastMeasuredTime = System.currentTimeMillis(); + LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM); + + + } @Override public void exec() { + + if (System.currentTimeMillis() - lastMeasuredTime > 150) { + // checking to see if time is greater than 150 milliseconds + lastMeasuredTime = System.currentTimeMillis(); + + currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM); + + if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) { + // I am moving forward + // and im close too my target. + LastDistanceRead = currentDistance; + + } else { + // I have stopped + targetDrivePower = 0; +// setHasFinished(true); + } + } + if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java new file mode 100644 index 0000000..4255cb7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -0,0 +1,98 @@ +package org.timecrafters.Autonomous.States; + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.testing.states.PrototypeBot1; + +import java.util.List; + +public class ConeIdentification extends CyberarmState { + PrototypeBot1 robot; + private int time; + private float minimumConfidence; + private int ParkPlace; + + public ConeIdentification(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + minimumConfidence = robot.configuration.variable(groupName, actionName, "Minimum Confidence").value(); + time = robot.configuration.variable(groupName, actionName, "time").value(); + } + + @Override + public void init() { + engine.blackboard.put("parkPlace", "1"); + robot.tfod.activate(); + } + + @Override + public void telemetry() { + if (robot.tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.tfod.getUpdatedRecognitions(); + if (updatedRecognitions != null) { + engine.telemetry.addData("# Objects Detected", updatedRecognitions.size()); + + // step through the list of recognitions and display image position/size information for each one + // Note: "Image number" refers to the randomized image orientation/number + for (Recognition recognition : updatedRecognitions) { + double col = (recognition.getLeft() + recognition.getRight()) / 2 ; + double row = (recognition.getTop() + recognition.getBottom()) / 2 ; + double width = Math.abs(recognition.getRight() - recognition.getLeft()) ; + double height = Math.abs(recognition.getTop() - recognition.getBottom()) ; + + engine.telemetry.addData("Label", recognition.getLabel()); + engine.telemetry.addData(""," "); + engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 ); + engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col); + engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); + + if (recognition.getLabel().equals("2 Bulb")) { + engine.telemetry.addData("2 Bulb", engine.blackboard.put("parkPlace", "2")); + } else if (recognition.getLabel().equals("3 Panel")) { + engine.telemetry.addData("3 Panel",engine.blackboard.put("parkPlace", "3")); + } else { + engine.telemetry.addData("1 Bolt", engine.blackboard.put("parkPlace", "1")); + } + } + } + } + } + + @Override + public void exec() { + + if (robot.tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.tfod.getUpdatedRecognitions(); + if (updatedRecognitions != null) { + // step through the list of recognitions and display image position/size information for each one + // Note: "Image number" refers to the randomized image orientation/number + float bestConfidence = 0; + + for (Recognition recognition : updatedRecognitions) { + if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) { + bestConfidence = recognition.getConfidence(); + + if (recognition.getLabel().equals("2 Bulb")) { + engine.blackboard.put("parkPlace", "2"); + } else if (recognition.getLabel().equals("3 Panel")) { + engine.blackboard.put("parkPlace", "3"); + + } else { + engine.blackboard.put("parkPlace", "1"); + } + } + } + } + } + + if (runTime() >= time){ + setHasFinished(true); + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java new file mode 100644 index 0000000..97c4ae4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -0,0 +1,39 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class PathDecision extends CyberarmState { + PrototypeBot1 robot; + private String groupName; + + + public PathDecision (PrototypeBot1 robot, String groupName, String actionName){ + this.robot = robot; + this.groupName = groupName; + + } + + @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")); + } + } + + } + + + + + + diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 13f1eda..24724ff 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -29,11 +29,11 @@ public class PrototypeBot1 { private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; - private VuforiaLocalizer vuforia; + public VuforiaLocalizer vuforia; - private TFObjectDetector tfod; + public TFObjectDetector tfod; - public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight; + public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo; private final CyberarmEngine engine; public Rev2mDistanceSensor collectorDistance; @@ -80,6 +80,9 @@ public class PrototypeBot1 { // servo configuration + //Camera Servo + CameraServo = engine.hardwareMap.servo.get("Camera Servo"); + // Collector collectorLeft = engine.hardwareMap.crservo.get("Collector Left"); collectorRight = engine.hardwareMap.crservo.get("Collector Right"); @@ -122,17 +125,21 @@ public class PrototypeBot1 { HighRiserLeft.setPosition(0.45); HighRiserRight.setPosition(0.45); + CameraServo.setPosition(0); + } private void initVuforia(){ /* * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. */ - VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = VUFORIA_KEY; parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + // Instantiate the Vuforia engine vuforia = ClassFactory.getInstance().createVuforia(parameters); } From f5805567e7aec6f7e08dea4861f84c71d62c48ea Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 5 Nov 2022 16:58:32 -0500 Subject: [PATCH 4/8] Added debugging telemetry to CollectorDistanceState that showed that there was most likely not a bug with the distance delta- but that the 2M distance sensor is flakey at long ranges, Fixed mecanum minibot unable to grab --- .../States/CollectorDistanceState.java | 33 ++++++++++++++----- .../states/MecanumMinibotTeleOpState.java | 4 +-- 2 files changed, 27 insertions(+), 10 deletions(-) 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 9fc3355..fc7e32a 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -17,6 +17,9 @@ public class CollectorDistanceState extends CyberarmState { private double lastMeasuredTime; private double currentDistance; private double LastDistanceRead; + private double distanceDelta; + private double debugRunTime; + private String debugStatus = "?"; public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { @@ -30,20 +33,30 @@ public class CollectorDistanceState extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM)); 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.addLine(); engine.telemetry.addData("traveledDistance", traveledDistance); engine.telemetry.addData("RampUpDistance", RampUpDistance); engine.telemetry.addData("RampDownDistance", RampDownDistance); + + engine.telemetry.addLine(); + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addLine(); + engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Current Distance", currentDistance); engine.telemetry.addData("last Distance", LastDistanceRead); + engine.telemetry.addLine(); + + engine.telemetry.addData("distanceDelta", distanceDelta); + engine.telemetry.addData("DEBUG RunTime", debugRunTime); + engine.telemetry.addData("DEBUG Status", debugStatus); } @@ -82,15 +95,19 @@ public class CollectorDistanceState extends CyberarmState { currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM); - if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) { + distanceDelta = LastDistanceRead - currentDistance; + + if (distanceDelta >= 0.0 || currentDistance > 500) { // I am moving forward // and im close too my target. LastDistanceRead = currentDistance; - + debugRunTime = runTime(); + debugStatus = "RUNNING"; } else { // I have stopped - targetDrivePower = 0; -// setHasFinished(true); + debugStatus = "STOPPED"; + setHasFinished(true); + return; } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java index 99890ae..3d46479 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java @@ -71,7 +71,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState { } /* ............................................................................ grab */ - if(engine.gamepad1.left_stick_x>0.5 || engine.gamepad1.right_stick_x<-0.5 || + if(engine.gamepad1.left_stick_x < -0.5 || engine.gamepad1.right_stick_x < -0.5 || engine.gamepad2.x){ // in robot.pServoGrab.setPosition(0.9); } @@ -79,7 +79,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState { engine.gamepad2.back){ // small out robot.pServoGrab.setPosition(0.50); } - if(engine.gamepad1.right_stick_x>0.5 || engine.gamepad1.right_stick_x>0.5 || + if(engine.gamepad1.left_stick_x > 0.5 || engine.gamepad1.right_stick_x > 0.5 || engine.gamepad2.b){ // big out robot.pServoGrab.setPosition(0.0); } From 6fe37971c68d013e7fef9b09ce952ddd402e7adb Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 5 Nov 2022 21:43:34 -0500 Subject: [PATCH 5/8] Updated TeleOp gamepad 2: Y button to raise up to 0.9 and dpad up to 1.0 --- .../org/timecrafters/testing/states/PrototypeTeleOPState.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java index 8fb0de3..29acf50 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java @@ -360,7 +360,7 @@ public class PrototypeTeleOPState extends CyberarmState { } if (engine.gamepad2.dpad_up) { - if (robot.HighRiserLeft.getPosition() < 0.9) { + if (robot.HighRiserLeft.getPosition() < 1.0) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); @@ -380,7 +380,7 @@ public class PrototypeTeleOPState extends CyberarmState { } if (engine.gamepad2.y) { - if (robot.HighRiserLeft.getPosition() < 0.84) { + if (robot.HighRiserLeft.getPosition() < 0.9) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); 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 6/8] 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); } } From 2da92450b57c234aa53eb66bdcd8202ed0ddf04e Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 7 Nov 2022 19:07:41 -0600 Subject: [PATCH 7/8] RightSideAutonomous has drive backward and forward states after rotating towards both of the junctions for allignment. --- .../Engines/RightSideAutonomousEngine.java | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index 6b0339c..5bcd405 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -33,6 +33,8 @@ public class RightSideAutonomousEngine extends CyberarmEngine { addState(new BottomArm(robot, "RightSideAutonomous", "04-0")); //drive forward addState(new DriverState(robot, "RightSideAutonomous", "05-0")); + // drive backward + addState(new DriverState(robot, "RightSideAutonomous", "05-1")); //lower the bottom arm to get closer addState(new TopArm(robot, "RightSideAutonomous", "06-0")); //make collector release the cone @@ -61,28 +63,30 @@ public class RightSideAutonomousEngine extends CyberarmEngine { addState(new DriverState(robot, "RightSideAutonomous", "17-0")); // rotate addState(new RotationState(robot, "RightSideAutonomous", "18-0")); - - addState(new DriverState(robot, "RightSideAutonomous", "21-0")); + // drive slightly towards junction + addState(new DriverState(robot, "RightSideAutonomous", "18-1")); + // drive away from junction + addState(new DriverState(robot, "RightSideAutonomous", "18-2")); // bring arm down. - addState(new TopArm(robot, "RightSideAutonomous", "22-0")); + addState(new TopArm(robot, "RightSideAutonomous", "19-0")); // get rid of cone - addState(new CollectorState(robot, "RightSideAutonomous", "23-0")); + addState(new CollectorState(robot, "RightSideAutonomous", "20-0")); // lift arm up to clear - addState(new TopArm(robot, "RightSideAutonomous", "24-0")); + addState(new TopArm(robot, "RightSideAutonomous", "21-0")); // rotate towards stack of cones - addState(new RotationState(robot, "RightSideAutonomous", "28-0")); + addState(new RotationState(robot, "RightSideAutonomous", "22-0")); // Choose Parking Spot - addState(new PathDecision(robot, "RightSideAutonomous", "29-0")); + addState(new PathDecision(robot, "RightSideAutonomous", "23-0")); // case 1 drive forward - addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-1")); + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-1")); // case 2 drive forward - addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-2")); + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-2")); // case 3 drive forward - addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-3")); + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-3")); // zero out - addState(new RotationState(robot, "RightSideAutonomous", "30-0")); + addState(new RotationState(robot, "RightSideAutonomous", "24-0")); // Top Arm Down - addState(new TopArm(robot, "RightSideAutonomous", "28-1")); + addState(new TopArm(robot, "RightSideAutonomous", "25-0")); } @Override From d21441eec53a8f7ff18a70867662f5d3b7e21ac4 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 7 Nov 2022 19:35:57 -0600 Subject: [PATCH 8/8] LeftSideAutonomous has drive backward and forward states after rotating towards both of the junctions for allignment. and it is organized --- .../Engines/LeftSideAutonomousEngine.java | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java index ebee709..fabe850 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -31,7 +31,7 @@ public class LeftSideAutonomousEngine extends CyberarmEngine { addState(new TopArm(robot, "LeftSideAutonomous", "03-0")); //lift the lower arm addState(new BottomArm(robot, "LeftSideAutonomous", "04-0")); - //drive forward + //drive forward to junction addState(new DriverState(robot, "LeftSideAutonomous", "05-0")); //lower the bottom arm to get closer addState(new TopArm(robot, "LeftSideAutonomous", "06-0")); @@ -39,14 +39,14 @@ public class LeftSideAutonomousEngine extends CyberarmEngine { addState(new CollectorState(robot, "LeftSideAutonomous", "07-0")); //lift the lower arm to clear the pole addState(new TopArm(robot, "LeftSideAutonomous", "08-0")); - //Drive Backwards + //Drive Backwards away from junction 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")); + addState(new RotationState(robot, "LeftSideAutonomous", "10-0")); // lower the bottom arm so we dont fall over - addState(new BottomArm(robot, "LeftSideAutonomous", "10-0")); + addState(new BottomArm(robot, "LeftSideAutonomous", "11-0")); // lower the upper arm so we dont fall over - addState(new TopArm(robot, "LeftSideAutonomous", "11-0"));; + addState(new TopArm(robot, "LeftSideAutonomous", "12-0"));; //adjust arm height to cone. addState(new TopArm(robot, "LeftSideAutonomous", "13-0")); //drive towards stack of cones while collecting @@ -59,32 +59,32 @@ public class LeftSideAutonomousEngine extends CyberarmEngine { addState(new TopArm(robot, "LeftSideAutonomous", "16-0")); // drive backwards too position addState(new DriverState(robot, "LeftSideAutonomous", "17-0")); - // rotate + // face low junction addState(new RotationState(robot, "LeftSideAutonomous", "18-0")); - - addState(new DriverState(robot, "LeftSideAutonomous", "21-0")); + // drive up to junction + addState(new DriverState(robot, "LeftSideAutonomous", "19-0")); // bring arm down. - addState(new TopArm(robot, "LeftSideAutonomous", "22-0")); + addState(new TopArm(robot, "LeftSideAutonomous", "20-0")); // get rid of cone - addState(new CollectorState(robot, "LeftSideAutonomous", "23-0")); + addState(new CollectorState(robot, "LeftSideAutonomous", "21-0")); // lift arm up to clear - addState(new TopArm(robot, "LeftSideAutonomous", "24-0")); + addState(new TopArm(robot, "LeftSideAutonomous", "22-0")); // rotate towards stack of cones - addState(new RotationState(robot, "LeftSideAutonomous", "28-0")); + addState(new RotationState(robot, "LeftSideAutonomous", "23-0")); // Choose Parking Spot - addState(new PathDecision(robot, "LeftSideAutonomous", "29-0")); + addState(new PathDecision(robot, "LeftSideAutonomous", "24-0")); // case 1 drive forward - addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-1")); + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-1")); // case 2 drive forward - addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-2")); + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-2")); // case 3 drive forward - addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-3")); + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-3")); // zero out - addState(new RotationState(robot, "LeftSideAutonomous", "30-0")); + addState(new RotationState(robot, "LeftSideAutonomous", "25-0")); // Top Arm Down - addState(new TopArm(robot, "LeftSideAutonomous", "28-1")); + addState(new TopArm(robot, "LeftSideAutonomous", "25-1")); //Drive Backwards - addState(new DriverState(robot, "LeftSideAutonomous", "30-1")); + addState(new DriverState(robot, "LeftSideAutonomous", "26-0")); } @Override