From 7f7ebf6d205129c8fe8d88e73aa463183f93f0d8 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Tue, 17 Jan 2023 20:01:01 -0600 Subject: [PATCH 1/2] autonomous work right side almost done --- .../Engines/LeftSideAutonomousEngine.java | 2 + .../Engines/LeftTwoConeAutonomousEngine.java | 186 ++++++++++++++++++ .../Engines/RightSideAutonomousEngine.java | 2 + ...java => RightTwoConeAutonomousEngine.java} | 119 +++++------ .../Autonomous/States/ConeIdentification.java | 4 +- .../States/DriverParkPlaceState.java | 11 +- .../States/DriverStateWithOdometer.java | 10 +- .../Autonomous/States/RotationState.java | 8 +- .../TeleOp/states/PhoenixBot1.java | 8 +- .../TeleOp/states/PhoenixTeleOPState.java | 22 +-- build.dependencies.gradle | 1 + 11 files changed, 290 insertions(+), 83 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java rename TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/{RightFourConeAutonomousEngine.java => RightTwoConeAutonomousEngine.java} (50%) 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 38559f1..d4e0451 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -1,6 +1,7 @@ package org.timecrafters.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.Autonomous.States.BottomArm; @@ -16,6 +17,7 @@ import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.TeleOp.states.PhoenixBot1; @Autonomous (name = "Left Side") +@Disabled public class LeftSideAutonomousEngine extends CyberarmEngine { PhoenixBot1 robot; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java new file mode 100644 index 0000000..bab0c18 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java @@ -0,0 +1,186 @@ +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.DriverStateWithOdometer; +import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState; +import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState; +import org.timecrafters.Autonomous.States.PathDecision; +import org.timecrafters.Autonomous.States.RotationState; +import org.timecrafters.Autonomous.States.ServoCameraRotate; +import org.timecrafters.Autonomous.States.TopArm; +import org.timecrafters.TeleOp.states.PhoenixBot1; + +@Autonomous (name = "left 2 cone auto") + +public class LeftTwoConeAutonomousEngine extends CyberarmEngine { + PhoenixBot1 robot; + + @Override + public void setup() { + robot = new PhoenixBot1(this); + + // 1 Rotate camera down at the signal + addState(new ServoCameraRotate(robot, "LeftTwoCone", "01-0")); + + // 2 Scan custom image + addState(new ConeIdentification(robot, "LeftTwoCone", "02-0")); + + // 3 Rotate Camera up, out of the way so it doesn't crash into stuff + addState(new ServoCameraRotate(robot, "LeftTwoCone", "03-0")); + + // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "04-0")); +// addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "04-1")); + addState(new TopArm(robot, "LeftTwoCone", "04-1")); + // 6 Raise lower arm while slowly driving at the junction (parallel state) + addState(new BottomArm(robot, "LeftTwoCone", "05-0")); + + // 6 Turn Towards and look for junction with sensor + addState(new RotationState(robot, "LeftTwoCone", "06-0")); + + // 6-1 drive forward or backwards if needed this is customizable so we can adapt + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "06-1")); + + // 6-3 align to junction with rotation or skip if it looks like it won't be able to + addState(new JunctionAllignmentDistanceState(robot, "LeftTwoCone", "06-3")); + addState(new JunctionAllignmentAngleState(robot, "LeftTwoCone", "06-4")); + + //pause + addState(new BottomArm(robot, "LeftTwoCone", "06-2")); + + + // 7 Drop bottom arm down on the junction to place cone + addState(new BottomArm(robot, "LeftTwoCone", "07-0")); + addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "07-1")); + + // 7-1 drive back slightly + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "07-2")); + + // 8 Drop cone as soon as arm is in position + addState(new CollectorState(robot, "LeftTwoCone", "08-0")); + // 8-1 drive back to lose contact + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "08-1")); + // 8-1 realign to old angle + addState(new RotationState(robot, "LeftTwoCone", "08-2")); + + // 9 Raise bottom arm to clear junction + addParallelStateToLastState(new BottomArm(robot, "LeftTwoCone", "09-0")); + addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "09-1")); + +// // 10 Back up and bring lower arm down (parallel state) + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "10-0")); + addParallelStateToLastState(new BottomArm(robot, "LeftTwoCone", "10-1")); + + // 11 Rotate towards stack + addState(new RotationState(robot, "LeftTwoCone", "11-0")); +// + // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor) + addState(new TopArm(robot, "LeftTwoCone", "12-0")); + + // drive forward at the stack without sensor + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "12-1")); + + // turn and align at stack + addState(new RotationState(robot, "LeftTwoCone", "12-2")); + +// + // 13 Drive at stack while collecting and check to see when we grab it + addState(new CollectorDistanceState(robot, "LeftTwoCone", "13-0")); + + +// +// // 14 Back up and raise arm + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "14-0")); + addState(new TopArm(robot, "LeftTwoCone", "14-1")); + + // 14-2 align perpendicular too the wall + addState(new RotationState(robot, "LeftTwoCone", "14-2")); +// + // 15 Drive All the way back to the tall Junction and raise upper arm (parallel state) + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "15-0")); +// + // 16 Rotate and use sensor to find junction + addState(new RotationState(robot, "LeftTwoCone", "16-0")); +// + // 17 Drive Towards Junction (This is optional, idk if this is needed atm) + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "17-0")); + +// // 18 Bring upper arm down + addState(new TopArm(robot, "LeftTwoCone", "18-0")); +// +// // 19 Drop cone + addState(new CollectorState(robot, "LeftTwoCone", "19-0")); + + addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "19-1")); + +// +// // 20 Bring upper arm up +// addState(new TopArm(robot, "LeftTwoCone", "20-0")); +// +// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier) + addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "21-0")); +// +// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state) +// addState(new TopArm(robot, "LeftTwoCone", "22-0")); +// +// // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack +// addState(new CollectorDistanceState(robot, "LeftTwoCone", "23-0")); +// +// // 24 Drive Back and lift up all the way to position for the low junction +// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "24-0")); +// addState(new TopArm(robot, "LeftTwoCone", "24-1")); +// +// // 25 Drive back faster after the cone is fully off of the stack +// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "25-0")); +// +// // 26 Turn and look for the low junction with the distance sensor and align +// addState(new RotationState(robot, "LeftTwoCone", "26-0")); +// +// // 27 Drive forward / backwards if you need to. (check with distance sensor) +// addState(new JunctionAllignmentState(robot, "LeftTwoCone", "26-1")); +// +// // 28 Bring Upper arm down on junction +// addState(new TopArm(robot, "LeftTwoCone", "28-0")); +// +// // 29 Let go of cone right after arm is in position +// addState(new CollectorState(robot, "LeftTwoCone", "29-0")); +// +// // 30 Raise arm as soon as the cone is dropped +// addState(new TopArm(robot, "LeftTwoCone", "30-0")); +// +// // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction +// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "31-0")); +// +// // 32 Rotate towards Stack of cones + addState(new RotationState(robot, "LeftTwoCone", "32-0")); +// +// // 33 Decide which path after scanning image from earlier + addState(new PathDecision(robot, "LeftTwoCone", "33-0")); +// +// // 34 Drive backwards, forwards, or stay put + addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-1")); + addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-2")); + addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-3")); +// +// // 35 Rotate towards alliance terminal + addState(new RotationState(robot, "LeftTwoCone", "35-0")); + + addState(new TopArm(robot, "RightTwoCone", "36-0")); + + + } + + @Override + public void loop() { + super.loop(); + + telemetry.addData("BlackBoard Input", blackboardGetString("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 index fef79d6..e2c0254 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -1,6 +1,7 @@ package org.timecrafters.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.Autonomous.States.CollectorDistanceState; @@ -15,6 +16,7 @@ import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.TeleOp.states.PhoenixBot1; @Autonomous (name = "Right Side") +@Disabled public class RightSideAutonomousEngine extends CyberarmEngine { PhoenixBot1 robot; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java similarity index 50% rename from TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java index 7705b27..9d16545 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java @@ -7,6 +7,7 @@ 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.DriverStateWithOdometer; import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState; import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState; @@ -16,9 +17,9 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.TeleOp.states.PhoenixBot1; -@Autonomous (name = "4 cone auto right") +@Autonomous (name = "Right 2 cone auto") -public class RightFourConeAutonomousEngine extends CyberarmEngine { +public class RightTwoConeAutonomousEngine extends CyberarmEngine { PhoenixBot1 robot; @Override @@ -26,147 +27,153 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { robot = new PhoenixBot1(this); // 1 Rotate camera down at the signal - addState(new ServoCameraRotate(robot, "RightFourCone", "01-0")); + addState(new ServoCameraRotate(robot, "RightTwoCone", "01-0")); // 2 Scan custom image - addState(new ConeIdentification(robot, "RightFourCone", "02-0")); + addState(new ConeIdentification(robot, "RightTwoCone", "02-0")); // 3 Rotate Camera up, out of the way so it doesn't crash into stuff - addState(new ServoCameraRotate(robot, "RightFourCone", "03-0")); + addState(new ServoCameraRotate(robot, "RightTwoCone", "03-0")); // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel - addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0")); -// addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1")); - addState(new TopArm(robot, "RightFourCone", "04-1")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "04-0")); +// addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "04-1")); + addState(new TopArm(robot, "RightTwoCone", "04-1")); // 6 Raise lower arm while slowly driving at the junction (parallel state) - addState(new BottomArm(robot, "RightFourCone", "05-0")); + addState(new BottomArm(robot, "RightTwoCone", "05-0")); // 6 Turn Towards and look for junction with sensor - addState(new RotationState(robot, "RightFourCone", "06-0")); + addState(new RotationState(robot, "RightTwoCone", "06-0")); // 6-1 drive forward or backwards if needed this is customizable so we can adapt - addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "06-1")); // 6-3 align to junction with rotation or skip if it looks like it won't be able to - addState(new JunctionAllignmentDistanceState(robot, "RightFourCone", "06-3")); - addState(new JunctionAllignmentAngleState(robot, "RightFourCone", "06-4")); + addState(new JunctionAllignmentDistanceState(robot, "RightTwoCone", "06-3")); + addState(new JunctionAllignmentAngleState(robot, "RightTwoCone", "06-4")); //pause - addState(new BottomArm(robot, "RightFourCone", "06-2")); + addState(new BottomArm(robot, "RightTwoCone", "06-2")); // 7 Drop bottom arm down on the junction to place cone - addState(new BottomArm(robot, "RightFourCone", "07-0")); - addParallelStateToLastState(new TopArm(robot, "RightFourCone", "07-1")); + addState(new BottomArm(robot, "RightTwoCone", "07-0")); + addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "07-1")); // 7-1 drive back slightly - addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-2")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "07-2")); // 8 Drop cone as soon as arm is in position - addState(new CollectorState(robot, "RightFourCone", "08-0")); + addState(new CollectorState(robot, "RightTwoCone", "08-0")); // 8-1 drive back to lose contact - addState(new DriverStateWithOdometer(robot, "RightFourCone", "08-1")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "08-1")); // 8-1 realign to old angle - addState(new RotationState(robot, "RightFourCone", "08-2")); + addState(new RotationState(robot, "RightTwoCone", "08-2")); // 9 Raise bottom arm to clear junction - addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "09-0")); - addParallelStateToLastState(new TopArm(robot, "RightFourCone", "09-1")); + addParallelStateToLastState(new BottomArm(robot, "RightTwoCone", "09-0")); + addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "09-1")); // // 10 Back up and bring lower arm down (parallel state) - addState(new DriverStateWithOdometer(robot, "RightFourCone", "10-0")); - addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "10-1")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "10-0")); + addParallelStateToLastState(new BottomArm(robot, "RightTwoCone", "10-1")); // 11 Rotate towards stack - addState(new RotationState(robot, "RightFourCone", "11-0")); + addState(new RotationState(robot, "RightTwoCone", "11-0")); // // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor) - addState(new TopArm(robot, "RightFourCone", "12-0")); + addState(new TopArm(robot, "RightTwoCone", "12-0")); // drive forward at the stack without sensor - addState(new DriverStateWithOdometer(robot, "RightFourCone", "12-1")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "12-1")); // turn and align at stack - addState(new RotationState(robot, "RightFourCone", "12-2")); + addState(new RotationState(robot, "RightTwoCone", "12-2")); // // 13 Drive at stack while collecting and check to see when we grab it - addState(new CollectorDistanceState(robot, "RightFourCone", "13-0")); + addState(new CollectorDistanceState(robot, "RightTwoCone", "13-0")); // // // 14 Back up and raise arm - addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0")); - addState(new TopArm(robot, "RightFourCone", "14-1")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "14-0")); + addState(new TopArm(robot, "RightTwoCone", "14-1")); // 14-2 align perpendicular too the wall - addState(new RotationState(robot, "RightFourCone", "14-2")); + addState(new RotationState(robot, "RightTwoCone", "14-2")); // // 15 Drive All the way back to the tall Junction and raise upper arm (parallel state) - addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "15-0")); // // 16 Rotate and use sensor to find junction - addState(new RotationState(robot, "RightFourCone", "16-0")); + addState(new RotationState(robot, "RightTwoCone", "16-0")); // // 17 Drive Towards Junction (This is optional, idk if this is needed atm) - addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "17-0")); // // 18 Bring upper arm down - addState(new TopArm(robot, "RightFourCone", "18-0")); + addState(new TopArm(robot, "RightTwoCone", "18-0")); // // // 19 Drop cone - addState(new CollectorState(robot, "RightFourCone", "19-0")); + addState(new CollectorState(robot, "RightTwoCone", "19-0")); + + addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "19-1")); + // // // 20 Bring upper arm up -// addState(new TopArm(robot, "RightFourCone", "20-0")); +// addState(new TopArm(robot, "RightTwoCone", "20-0")); // // // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier) - addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0")); + addState(new DriverStateWithOdometer(robot, "RightTwoCone", "21-0")); // // // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state) -// addState(new TopArm(robot, "RightFourCone", "22-0")); +// addState(new TopArm(robot, "RightTwoCone", "22-0")); // // // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack -// addState(new CollectorDistanceState(robot, "RightFourCone", "23-0")); +// addState(new CollectorDistanceState(robot, "RightTwoCone", "23-0")); // // // 24 Drive Back and lift up all the way to position for the low junction -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0")); -// addState(new TopArm(robot, "RightFourCone", "24-1")); +// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "24-0")); +// addState(new TopArm(robot, "RightTwoCone", "24-1")); // // // 25 Drive back faster after the cone is fully off of the stack -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0")); +// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "25-0")); // // // 26 Turn and look for the low junction with the distance sensor and align -// addState(new RotationState(robot, "RightFourCone", "26-0")); +// addState(new RotationState(robot, "RightTwoCone", "26-0")); // // // 27 Drive forward / backwards if you need to. (check with distance sensor) -// addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1")); +// addState(new JunctionAllignmentState(robot, "RightTwoCone", "26-1")); // // // 28 Bring Upper arm down on junction -// addState(new TopArm(robot, "RightFourCone", "28-0")); +// addState(new TopArm(robot, "RightTwoCone", "28-0")); // // // 29 Let go of cone right after arm is in position -// addState(new CollectorState(robot, "RightFourCone", "29-0")); +// addState(new CollectorState(robot, "RightTwoCone", "29-0")); // // // 30 Raise arm as soon as the cone is dropped -// addState(new TopArm(robot, "RightFourCone", "30-0")); +// addState(new TopArm(robot, "RightTwoCone", "30-0")); // // // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0")); +// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "31-0")); // // // 32 Rotate towards Stack of cones - addState(new RotationState(robot, "RightFourCone", "32-0")); + addState(new RotationState(robot, "RightTwoCone", "32-0")); // // // 33 Decide which path after scanning image from earlier - addState(new PathDecision(robot, "RightFourCone", "33-0")); + addState(new PathDecision(robot, "RightTwoCone", "33-0")); // // // 34 Drive backwards, forwards, or stay put - addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1")); - addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2")); - addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3")); + addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-1")); + addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-2")); + addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-3")); // // // 35 Rotate towards alliance terminal -// addState(new RotationState(robot, "RightFourCone", "35-0")); + addState(new RotationState(robot, "RightTwoCone", "35-0")); + + addState(new TopArm(robot, "RightTwoCone", "36-0")); + } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java index 43c6473..dc64fc8 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -89,9 +89,9 @@ public class ConeIdentification extends CyberarmState { if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) { bestConfidence = recognition.getConfidence(); - if (recognition.getLabel().equals("2 Bulb")) { + if (recognition.getLabel().equals("#2")) { engine.blackboardSet("parkPlace", "2"); - } else if (recognition.getLabel().equals("3 Panel")) { + } else if (recognition.getLabel().equals("#3")) { engine.blackboardSet("parkPlace", "3"); } else { diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java index 382615f..69e585a 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -49,12 +49,12 @@ public class DriverParkPlaceState extends CyberarmState { setHasFinished(true); } if (placement.equals(intendedPlacement)) { - double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + double delta = traveledDistance - Math.abs(robot.OdometerEncoder.getCurrentPosition()); - if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) { + if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) <= RampUpDistance) { // ramping up - drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; - } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) { + drivePower = (Math.abs((float) robot.OdometerEncoder.getCurrentPosition()) / RampUpDistance) + 0.25; + } else if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) >= delta) { // ramping down drivePower = ((delta / RampDownDistance) + 0.25); } else { @@ -71,7 +71,7 @@ public class DriverParkPlaceState extends CyberarmState { drivePower = drivePower * -1; } - if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) { + if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) { robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); @@ -96,6 +96,7 @@ public class DriverParkPlaceState extends CyberarmState { engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoder.getCurrentPosition()); engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("targetDrivePower", targetDrivePower); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index acd6ad7..010b2e8 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -15,6 +15,7 @@ public class DriverStateWithOdometer extends CyberarmState { private int maximumTolerance; private float direction; private boolean targetAchieved = false; + private double CurrentPosition; public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); @@ -36,12 +37,14 @@ public class DriverStateWithOdometer extends CyberarmState { robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.OdometerEncoderLeft.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); robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -55,7 +58,12 @@ public class DriverStateWithOdometer extends CyberarmState { return; } - double CurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition()); + double RightCurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition()); + double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition()); + + if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition; + if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition; + if (Math.abs(CurrentPosition) <= RampUpDistance){ // ramping up 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 e5526b3..86ac710 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -35,12 +35,12 @@ public class RotationState extends CyberarmState { RobotRotation = robot.imu.getAngularOrientation().firstAngle; if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){ - drivePowerVariable = 0.3 * drivePower; - if (Math.abs(drivePowerVariable) < 0.3) { + drivePowerVariable = 0.4 * drivePower; + if (Math.abs(drivePowerVariable) < 0.4) { if (drivePowerVariable < 0){ - drivePowerVariable = -0.3; + drivePowerVariable = -0.4; } else { - drivePowerVariable = 0.3; + drivePowerVariable = 0.4; } } debugStatus = "Rotate Slow"; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index 09ab5d0..579a10d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -44,7 +44,7 @@ public class PhoenixBot1 { public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo; private final CyberarmEngine engine; - public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance; + public Rev2mDistanceSensor collectorDistance, /*downSensor,*/ leftPoleDistance, rightPoleDistance; public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft; @@ -104,7 +104,7 @@ public class PhoenixBot1 { collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); - downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance"); +// downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance"); leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance"); rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance"); @@ -195,8 +195,8 @@ public class PhoenixBot1 { LowRiserLeft.setPosition(0.35); LowRiserRight.setPosition(0.35); - HighRiserLeft.setPosition(0.45); - HighRiserRight.setPosition(0.45); + HighRiserLeft.setPosition(0.40); + HighRiserRight.setPosition(0.40); CameraServo.setPosition(0.775); diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 7874452..5098fed 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -58,7 +58,7 @@ public class PhoenixTeleOPState extends CyberarmState { engine.telemetry.addData("Drive Power", drivePower); engine.telemetry.addData("Delta Rotation", DeltaRotation); engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM)); - engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM)); +// engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Odometer Encoder, Right", robot.OdometerEncoder.getCurrentPosition()); @@ -483,15 +483,15 @@ public class PhoenixTeleOPState extends CyberarmState { robot.imu.initialize(parameters); } } - public double downSensor() { - double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5; - Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM); - Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM); - Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM); - Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM); - Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM); - Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5; - return Distance; +// public double downSensor() { +// double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5; +// Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM); +// Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM); +// Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM); +// Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM); +// Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM); +// Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5; +// return Distance; } -} \ No newline at end of file + diff --git a/build.dependencies.gradle b/build.dependencies.gradle index eeab776..cc5dfab 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -18,5 +18,6 @@ dependencies { implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'org.firstinspires.ftc:gameAssets-PowerPlay:1.0.0' + implementation 'com.acmerobotics.roadrunner:core:0.5.1' } From 6ae272003e7f2f2ea8f9c1c2629288292f1d311e Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Thu, 19 Jan 2023 13:08:47 -0600 Subject: [PATCH 2/2] Fixed broken build due to imcomplete addition of RoadRunner, Refactored minibots out of my package..., added initial implementation of CHIRON minibot. --- .idea/jarRepositories.xml | 5 + TeamCode/build.gradle | 6 + .../TeleOp/states/LaserState.java | 2 +- .../minibots/{cyberarm => }/Engine.java | 4 +- .../{cyberarm => }/MecanumMinibot.java | 2 +- .../{cyberarm => }/PortsCheckGeneric.java | 2 +- .../minibots/{cyberarm => }/State.java | 4 +- .../minibots/cyberarm/chiron/Robot.java | 266 ++++++++++++++++ .../cyberarm/chiron/engines/TeleOpEngine.java | 23 ++ .../chiron/states/DriverControlState.java | 284 ++++++++++++++++++ .../cyberarm/states/BlitzkriegState.java | 4 - .../{cyberarm => }/engines/Common.java | 2 +- .../engines/FieldOrientedEngine.java | 7 +- .../engines/MecanumMinibotTeleOpEngine.java | 6 +- .../engines/Mecanum_Robot_Engine.java | 6 +- .../engines/Mecanum_fancy_drive_TeleOp.java | 6 +- .../engines/PingPongEngine.java | 6 +- .../{cyberarm => }/engines/TasksEngine.java | 2 +- .../{cyberarm => }/engines/pickle_engine.java | 6 +- .../pickle_minibot_general.java | 2 +- .../{cyberarm => }/states/.editorconfig | 0 .../states/AutonomousReversalExperiment.java | 44 +-- .../minibots/states/BlitzkriegState.java | 4 + .../{cyberarm => }/states/DriveState.java | 0 .../states/FieldOrientedDrive.java | 134 ++++----- .../states/MecanumMinibotTeleOpState.java | 4 +- .../{cyberarm => }/states/MecanumRobot.java | 100 +++--- .../states/Mecanum_Fancy_Drive_State.java | 209 +++++++------ .../states/Mecanum_Robot_State.java | 234 +++++++-------- .../{cyberarm => }/states/PingPongState.java | 70 ++--- .../states/pickle_teleop_state.java | 4 +- build.common.gradle | 6 - build.dependencies.gradle | 1 + build.gradle | 2 +- 34 files changed, 1017 insertions(+), 440 deletions(-) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/Engine.java (76%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/MecanumMinibot.java (99%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/PortsCheckGeneric.java (73%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/State.java (93%) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/BlitzkriegState.java rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/Common.java (89%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/FieldOrientedEngine.java (61%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/MecanumMinibotTeleOpEngine.java (68%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/Mecanum_Robot_Engine.java (66%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/Mecanum_fancy_drive_TeleOp.java (69%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/PingPongEngine.java (66%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/TasksEngine.java (97%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/engines/pickle_engine.java (68%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/pickle_minibot_general.java (96%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/.editorconfig (100%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/AutonomousReversalExperiment.java (83%) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/states/BlitzkriegState.java rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/DriveState.java (100%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/FieldOrientedDrive.java (95%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/MecanumMinibotTeleOpState.java (94%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/MecanumRobot.java (94%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/Mecanum_Fancy_Drive_State.java (95%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/Mecanum_Robot_State.java (96%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/PingPongState.java (93%) rename TeamCode/src/main/java/org/timecrafters/minibots/{cyberarm => }/states/pickle_teleop_state.java (93%) diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml index 0380d8d..5c10211 100644 --- a/.idea/jarRepositories.xml +++ b/.idea/jarRepositories.xml @@ -26,5 +26,10 @@