From ec14347e0644278009e18012b5a494907e105ae2 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 22 Dec 2022 20:35:08 -0600 Subject: [PATCH] Autonomous work --- .../RightFourConeAutonomousEngine.java | 27 +++++++++++-------- .../Autonomous/States/BottomArm.java | 2 +- .../Autonomous/States/CollectorState.java | 4 +-- .../States/DriverStateWithOdometer.java | 3 +++ ...eWithOdometerLowerArmParallelState2nd.java | 7 ++++- .../Autonomous/States/ServoCameraRotate.java | 2 +- 6 files changed, 29 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java index 4d8555a..34dbdf9 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -3,6 +3,7 @@ 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; @@ -18,7 +19,7 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.TeleOp.states.PhoenixBot1; -@Autonomous (name = "3 cone auto right") +@Autonomous (name = "4 cone auto right") public class RightFourConeAutonomousEngine extends CyberarmEngine { PhoenixBot1 robot; @@ -39,23 +40,27 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel addState(new DriverStateWithOdometerUpperArmParallelState(robot, "RightFourCone", "04-0")); - // 5 Turn Towards and look for junction with sensor - addState(new RotationState(robot, "RightFourCone", "05-0")); - // 6 Raise lower arm while slowly driving at the junction (parallel state) - addState(new JunctionAllignmentState(robot, "RightFourCone", "06-0")); + addState(new BottomArm(robot, "RightFourCone", "05-0")); - // 7 Drop top arm down on the junction to place cone - addState(new TopArm(robot, "RightFourCone", "07-0")); + // 6 Turn Towards and look for junction with sensor + addState(new RotationState(robot, "RightFourCone", "06-0")); + + // 6-1 drive forward or backwards if needed this is customizable so we can adapt + addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1")); + + + // 7 Drop bottom arm down on the junction to place cone + addState(new BottomArm(robot, "RightFourCone", "07-0")); // 8 Drop cone as soon as arm is in position -// addState(new CollectorState(robot, "RightFourCone", "08-0")); + addState(new CollectorState(robot, "RightFourCone", "08-0")); - // 9 Raise arm to clear junction -// addState(new TopArm(robot, "RightFourCone", "09-0")); + // 9 Raise bottom arm to clear junction + addState(new BottomArm(robot, "RightFourCone", "09-0")); // // // 10 Back up and bring lower arm down (parallel state) -// addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0")); + addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0")); // // // 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor) // addState(new TopArm(robot, "RightFourCone", "11-0")); 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 9f08bfd..0975501 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -53,7 +53,7 @@ public class BottomArm extends CyberarmState { robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); - } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { + } if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java index 9740d47..b02757b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java @@ -54,8 +54,8 @@ public class CollectorState extends CyberarmState { } } else { - robot.collectorLeft.setPower(0); - robot.collectorRight.setPower(0); +// robot.collectorLeft.setPower(0); +// robot.collectorRight.setPower(0); setHasFinished(true); } 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 ca82a06..46cbf2f 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -30,11 +30,14 @@ public class DriverStateWithOdometer extends CyberarmState { 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.OdometerEncoder.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_USING_ENCODER); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java index 622fe32..9c4d2a5 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java @@ -30,17 +30,22 @@ public class DriverStateWithOdometerLowerArmParallelState2nd extends CyberarmSta 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.OdometerEncoder.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); + + + addParallelState(new BottomArm(robot, "RightFourCone", "10-1")); + } @Override public void exec() { - addParallelState(new BottomArm(robot, "RightFourCone", "10-1")); if (stateDisabled) { setHasFinished(true); return; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java index 6251720..d89327b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java @@ -21,7 +21,7 @@ public class ServoCameraRotate extends CyberarmState { setHasFinished(true); } else { robot.CameraServo.setPosition(ServoPosition); -// setHasFinished(true); + setHasFinished(true); } } }