From 825c0fa076e600cbdabbf8df168cc08cc570455e Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 9 Jan 2023 19:29:37 -0600 Subject: [PATCH] autonomous work --- .../RightFourConeAutonomousEngine.java | 8 +- .../States/CollectorDistanceState.java | 2 +- ...tateWithOdometerLowerArmParallelState.java | 112 --------- ...eWithOdometerLowerArmParallelState2nd.java | 116 --------- ...tateWithOdometerUpperArmParallelState.java | 144 ----------- .../States/JunctionAllignmentState.java | 225 +++++++++--------- .../Autonomous/States/RotationState.java | 4 + 7 files changed, 124 insertions(+), 487 deletions(-) delete mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.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 c6ccea2..9b4d1ee 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -7,13 +7,8 @@ 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.DriverState; import org.timecrafters.Autonomous.States.DriverStateWithOdometer; -import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState; -import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState2nd; -import org.timecrafters.Autonomous.States.DriverStateWithOdometerUpperArmParallelState; import org.timecrafters.Autonomous.States.JunctionAllignmentState; -import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; @@ -50,6 +45,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 6-1 drive forward or backwards if needed this is customizable so we can adapt 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")); + //pause addState(new BottomArm(robot, "RightFourCone", "06-2")); 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 545fe1a..00c5539 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -115,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState { distanceDelta = LastDistanceRead - currentDistance; - if (distanceDelta >= -50.0 || currentDistance > 500) { + if (distanceDelta >= -15.0 || currentDistance > 500) { // I am moving forward // and im close too my target. LastDistanceRead = currentDistance; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java deleted file mode 100644 index d4d7805..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java +++ /dev/null @@ -1,112 +0,0 @@ -package org.timecrafters.Autonomous.States; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; - -public class DriverStateWithOdometerLowerArmParallelState extends CyberarmState { - private final boolean stateDisabled; - PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; - private int maximumTolerance; - public DriverStateWithOdometerLowerArmParallelState(PhoenixBot1 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.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").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() { - - addParallelState(new BottomArm(robot, "RightFourCone", "06-1")); - if (stateDisabled) { - setHasFinished(true); - return; - } - - double CurrentPosition = robot.OdometerEncoder.getCurrentPosition(); - double delta = traveledDistance - Math.abs(CurrentPosition); - - if (Math.abs(CurrentPosition) <= RampUpDistance){ - // ramping up - drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25; - } - else if (Math.abs(CurrentPosition) >= 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(CurrentPosition) < traveledDistance - maximumTolerance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); - } else { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - 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("Odometer", robot.OdometerEncoder.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); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java deleted file mode 100644 index afe9718..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java +++ /dev/null @@ -1,116 +0,0 @@ -package org.timecrafters.Autonomous.States; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; - -public class DriverStateWithOdometerLowerArmParallelState2nd extends CyberarmState { - private final boolean stateDisabled; - PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; - private int maximumTolerance; - public DriverStateWithOdometerLowerArmParallelState2nd(PhoenixBot1 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.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").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.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); - - - - } - - @Override - public void exec() { - - if (stateDisabled) { - setHasFinished(true); - return; - } - - double CurrentPosition = robot.OdometerEncoder.getCurrentPosition(); - double delta = traveledDistance - Math.abs(CurrentPosition); - - if (Math.abs(CurrentPosition) <= RampUpDistance){ - // ramping up - drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25; - } - else if (Math.abs(CurrentPosition) >= 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(CurrentPosition) < traveledDistance - maximumTolerance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); - } else { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - 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("Odometer", robot.OdometerEncoder.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); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java deleted file mode 100644 index a6d4258..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java +++ /dev/null @@ -1,144 +0,0 @@ -package org.timecrafters.Autonomous.States; - -import android.util.Log; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; - -public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState { - private final boolean stateDisabled; - PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; - private int maximumTolerance; - public DriverStateWithOdometerUpperArmParallelState(PhoenixBot1 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.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").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); - - addParallelState(new TopArm(robot, "RightFourCone", "04-1")); - - } - - @Override - public void exec() { - - if (stateDisabled) { - setHasFinished(true); - return; - } - - double CurrentPosition = robot.OdometerEncoder.getCurrentPosition(); - double delta = traveledDistance - Math.abs(CurrentPosition); - - if (Math.abs(CurrentPosition) <= RampUpDistance){ - // ramping up - drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25; - } - else if (Math.abs(CurrentPosition) >= 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(CurrentPosition) < traveledDistance - maximumTolerance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); - } else { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - setHasFinished(true); - } - if (!getHasFinished()){ - float angle = robot.imu.getAngularOrientation().firstAngle; - - if (angle < -0.25){ - robot.backLeftDrive.setPower(drivePower * 0.25); - robot.backRightDrive.setPower(drivePower * 1.25); - robot.frontLeftDrive.setPower(drivePower * 0.25); - robot.frontRightDrive.setPower(drivePower * 1.25); - } - if (angle > 0.25){ - robot.backLeftDrive.setPower(drivePower * 1.25); - robot.backRightDrive.setPower(drivePower * 0.25); - robot.frontLeftDrive.setPower(drivePower * 1.25); - robot.frontRightDrive.setPower(drivePower * 0.25); - } - } - - - } - - @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("frontRightDrive", robot.frontRightDrive.getPower()); - engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower()); - engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower()); - engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower()); - engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition()); - engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle); - engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle); - engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle); - - - 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); - - Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle); - Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle); - Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java index 86a5e0b..3fcb3b6 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java @@ -7,15 +7,23 @@ import org.timecrafters.TeleOp.states.PhoenixBot1; public class JunctionAllignmentState extends CyberarmState { private final boolean stateDisabled; PhoenixBot1 robot; - private double TargetSensorDistance; - private final String targetedJunction; private final double drivePower; - private int whatToDo; + private int loopsTotal; + private float rotationAmount; + private float currentAngle; + private float targetAngle; + private float slop; + private double checkTime = 250; + + public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); - this.targetedJunction = robot.configuration.variable(groupName, actionName, "targetedJunction").value(); + this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value(); + this.rotationAmount = robot.configuration.variable(groupName, actionName, "rotationAmount").value(); + this.slop = robot.configuration.variable(groupName, actionName, "slop").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } @@ -23,133 +31,132 @@ public class JunctionAllignmentState extends CyberarmState { 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("target angle", targetAngle); + engine.telemetry.addData("current angle", currentAngle); + engine.telemetry.addData("drive power", drivePower); + + + + } + + @Override + public void start() { + } @Override public void exec() { + currentAngle = robot.imu.getAngularOrientation().firstAngle; + + if (stateDisabled){ setHasFinished(true); } else { double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM); double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM); + boolean rightInRange = rightDistance > 170 && rightDistance < 200; + boolean leftInRange = leftDistance > 170 && leftDistance < 200; - // TODO: 12/11/2022 Make sure these are the correct values for the distance from low, mid, and high junctions!!! - switch (targetedJunction) { - case "low": - TargetSensorDistance = 90.0; - break; - case "mid": - TargetSensorDistance = 135.0; - break; - case "high": - TargetSensorDistance = 200.0; - break; - } + // The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0 + + if (loopsTotal >= 5){ + setHasFinished(true); + } else if (runTime() - checkTime >= 250 ) { + + checkTime = runTime(); + loopsTotal = loopsTotal + 1; - // the state is finished if the distance sensors are at the correct distance. - if ((leftDistance > TargetSensorDistance - 2.0 || leftDistance < TargetSensorDistance + 2.0) && (rightDistance > TargetSensorDistance -2.0 || rightDistance < TargetSensorDistance + 2.0)) { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); + } else { + + if (rightInRange && leftInRange){ setHasFinished(true); + } + + if (rightInRange && !leftInRange) { + + // rotate 1 degree CW + + targetAngle = currentAngle + rotationAmount; + + if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) { + + robot.frontRightDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + + } else { + + if (targetAngle - slop < currentAngle) { + + robot.frontRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.backLeftDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + + } else if (targetAngle + slop > currentAngle) { + + robot.frontRightDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.backLeftDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(-drivePower); + + } + } -// whatToDo = (int)((leftDistance > TargetSensorDistance) << 1 ) + (int)(rightDistance > TargetSensorDistance); - - switch (whatToDo){ - case 0: // drive back - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); - break; - - case 1: // rotate CW - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - break; - - case 2: // rotate CCW - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - break; - - case 3: // Drive Forward - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - break; - - } - // the robot is lined up but needs to drive forward till the robot is at the specified distance - if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){ - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); + if (!rightInRange && leftInRange) { + + // rotate 1 degree CCW + + targetAngle = currentAngle - rotationAmount; + + if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) { + + robot.frontRightDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + + } else { + + if (targetAngle - slop < currentAngle) { + + robot.frontRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.backLeftDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + + } else if (targetAngle + slop > currentAngle) { + + robot.frontRightDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.backLeftDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(-drivePower); + + } + } - // the robot is lined up but needs to drive backward till the robot is at the specified distance - if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){ - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); - } - // the robot is going to rotate CCW until a distance is met - if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) { - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - } - // the robot is going to rotate CW until a distance is met - if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) { - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - } - // The right sensor is aligned but the robot must rotate CW with only the left side powered - if (leftDistance < TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) { - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(0); - } - // The right sensor is aligned but the robot must rotate rotate CCW with only the left side powered - if (leftDistance > TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) { - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(0); - } - // The left sensor is aligned but the robot must rotate CW with only the right side powered - if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance < TargetSensorDistance) { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(-drivePower); - } - // The left sensor is aligned but the robot must rotate CCW with only the right side powered - if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(drivePower); + + } + + if (!rightInRange && !leftInRange){ + + 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 dc60c74..e5526b3 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -81,6 +81,10 @@ public class RotationState extends CyberarmState { engine.telemetry.addData("Robot IMU Rotation", RobotRotation); engine.telemetry.addData("Robot Target Rotation", targetRotation); engine.telemetry.addData("Drive Power", drivePowerVariable); + 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()); } }