From a47223d6894c46d5a85e99a542dc0f6e8941e9ee Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 12 Jan 2023 20:34:42 -0600 Subject: [PATCH] autonomous work right side almost done --- .../RightFourConeAutonomousEngine.java | 18 ++- ...java => JunctionAllignmentAngleState.java} | 55 ++++--- .../JunctionAllignmentDistanceState.java | 144 ++++++++++++++++++ 3 files changed, 188 insertions(+), 29 deletions(-) rename TeamCode/src/main/java/org/timecrafters/Autonomous/States/{JunctionAllignmentState.java => JunctionAllignmentAngleState.java} (82%) create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java 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 335c8d7..7705b27 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -8,7 +8,8 @@ import org.timecrafters.Autonomous.States.CollectorDistanceState; import org.timecrafters.Autonomous.States.CollectorState; import org.timecrafters.Autonomous.States.ConeIdentification; import org.timecrafters.Autonomous.States.DriverStateWithOdometer; -import org.timecrafters.Autonomous.States.JunctionAllignmentState; +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; @@ -47,7 +48,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1")); // 6-3 align to junction with rotation or skip if it looks like it won't be able to - addState(new JunctionAllignmentState(robot, "RightFourCone", "06-3")); + addState(new JunctionAllignmentDistanceState(robot, "RightFourCone", "06-3")); + addState(new JunctionAllignmentAngleState(robot, "RightFourCone", "06-4")); //pause addState(new BottomArm(robot, "RightFourCone", "06-2")); @@ -55,16 +57,22 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 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")); // 7-1 drive back slightly - addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-1")); + addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-2")); // 8 Drop cone as soon as arm is in position addState(new CollectorState(robot, "RightFourCone", "08-0")); + // 8-1 drive back to lose contact + addState(new DriverStateWithOdometer(robot, "RightFourCone", "08-1")); + // 8-1 realign to old angle + addState(new RotationState(robot, "RightFourCone", "08-2")); // 9 Raise bottom arm to clear junction - addState(new BottomArm(robot, "RightFourCone", "09-0")); -// + addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "09-0")); + addParallelStateToLastState(new TopArm(robot, "RightFourCone", "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")); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java similarity index 82% rename from TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java index d12cd54..9e35d9c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java @@ -6,7 +6,7 @@ import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.TeleOp.states.PhoenixBot1; -public class JunctionAllignmentState extends CyberarmState { +public class JunctionAllignmentAngleState extends CyberarmState { private final boolean stateDisabled; PhoenixBot1 robot; private final double drivePower; @@ -25,7 +25,7 @@ public class JunctionAllignmentState extends CyberarmState { private boolean finishedEnabled; - public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) { + public JunctionAllignmentAngleState(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value(); @@ -114,6 +114,11 @@ public class JunctionAllignmentState extends CyberarmState { loopsCurrent = loopsCurrent + 1; if (rightInRange && leftInRange){ + + robot.frontRightDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.frontLeftDrive.setPower(0); setHasFinished(finishedEnabled); } @@ -188,30 +193,32 @@ public class JunctionAllignmentState extends CyberarmState { } - if (!rightInRange && !leftInRange && driveLoops < 2){ + if (!rightInRange && !leftInRange /*&& driveLoops < 2*/){ - if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) { - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); +// if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) { +// robot.backLeftDrive.setPower(drivePower); +// robot.backRightDrive.setPower(drivePower); +// robot.frontLeftDrive.setPower(drivePower); +// robot.frontRightDrive.setPower(drivePower); +// +// driveLoops += 1; +// } +// +// } else { +// +// robot.backLeftDrive.setPower(0); +// robot.backRightDrive.setPower(0); +// robot.frontLeftDrive.setPower(0); +// robot.frontRightDrive.setPower(0); +// +// robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// +// if (driveLoops == 2) { +// loopsCurrent = loopsTotal; +// } - driveLoops += 1; - } - - } else { - - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - - robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - if (driveLoops == 2) { - loopsCurrent = loopsTotal; - } + setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java new file mode 100644 index 0000000..c3b680a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java @@ -0,0 +1,144 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.TeleOp.states.PhoenixBot1; + +public class JunctionAllignmentDistanceState extends CyberarmState { + private final boolean stateDisabled; + PhoenixBot1 robot; + private final double drivePower; + private int loopsTotal; + private float slop; + private double checkTime = 250; + private int traveledDistance; + private int loopsCurrent; + private double minDistance; + private double maxDistance; + private String whereAmI = "init"; + private int driveLoops; + private boolean finishedEnabled; + double leftDistance; + double rightDistance; + boolean rightInRange; + boolean leftInRange; + + + public JunctionAllignmentDistanceState(PhoenixBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); + this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value(); + this.slop = robot.configuration.variable(groupName, actionName, "slop").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveled distance").value(); + this.minDistance = robot.configuration.variable(groupName, actionName, "minDistance").value(); + this.maxDistance = robot.configuration.variable(groupName, actionName, "maxDistance").value(); + this.finishedEnabled = robot.configuration.variable(groupName, actionName, "finishedEnabled").value(); + + + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } + + @Override + public void telemetry() { + engine.telemetry.addData("right sensor distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("left sensor distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("front right power", robot.frontRightDrive.getPower()); + engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower()); + engine.telemetry.addData("back left power", robot.backLeftDrive.getPower()); + engine.telemetry.addData("back right power", robot.backRightDrive.getPower()); + engine.telemetry.addData("drive power", drivePower); + engine.telemetry.addData("traveled distance", traveledDistance); + engine.telemetry.addData("Loops", loopsCurrent); + engine.telemetry.addData("where am i??", whereAmI); + engine.telemetry.addData("time", System.currentTimeMillis() - checkTime); + + + } + + @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.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); + + checkTime = System.currentTimeMillis() - 500; + + + loopsCurrent = 0; + + } + + @Override + public void exec() { + + + if (stateDisabled) { + setHasFinished(true); + } else { + + if (loopsCurrent >= loopsTotal ) { + + whereAmI = "exceeded loops"; + + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + setHasFinished(finishedEnabled); + + } else if (System.currentTimeMillis() - checkTime >= 100) { + + checkTime = System.currentTimeMillis(); + + loopsCurrent += 1; + + leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM); + rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM); + rightInRange = (rightDistance >= minDistance) && (rightDistance <= maxDistance); + leftInRange = (leftDistance >= minDistance) && (leftDistance <= maxDistance); + + if (rightInRange || leftInRange) { + + whereAmI = "in range"; + + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + setHasFinished(finishedEnabled); + + } + + else if (!rightInRange && !leftInRange) { + + whereAmI = "Out of range"; + +// if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) { + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + +// } + + } + + + + + } + } + } +}