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 176e917..91b1056 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,14 @@ 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; import org.timecrafters.TeleOp.states.PhoenixBot1; @Autonomous (name = "3 cone auto right") @@ -22,73 +29,113 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 1 Rotate camera down at the signal addState(new ServoCameraRotate(robot, "RightFourCone", "01-0")); + // 2 Scan custom image addState(new ConeIdentification(robot, "RightFourCone", "02-0")); + // 3 Rotate Camera up, out of the way so it doesn't crash into stuff addState(new ServoCameraRotate(robot, "RightFourCone", "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")); - // 5 Turn Towards and look for junction with sensor - // 6 Raise lower arm while slowly driving at the junction + // 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")); // 7 Drop top arm down on the junction to place cone + addState(new TopArm(robot, "RightFourCone", "07-0")); // 8 Drop cone as soon as arm is in position + addState(new CollectorState(robot, "RightFourCone", "08-0")); // 9 Raise arm to clear junction + addState(new TopArm(robot, "RightFourCone", "09-0")); // 10 Back up and bring lower arm down (parallel state) + 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")); - // 12 Rotate towards stack (this might be parallel with last step) + // 12 Rotate towards stack + //filled in as parallel state. in parallel with previous state // 13 Drive at stack while collecting and check to see when we grab it + addState(new CollectorDistanceState(robot, "RightFourCone", "13-0")); - // 14 Back up and raise arm (in parallel state) + // 14 Back up and raise arm + addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0")); + addState(new TopArm(robot, "RightFourCone", "14-1")); // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state) + addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0")); // 16 Rotate and use sensor to find junction + addState(new RotationState(robot, "RightFourCone", "16-0")); + addState(new JunctionAllignmentState(robot, "RightFourCone", "16-1")); // 17 Drive Towards Junction (This is optional, idk if this is needed atm) + addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0")); // 18 Bring upper arm down + addState(new TopArm(robot, "RightFourCone", "18-0")); // 19 Drop cone + addState(new CollectorState(robot, "RightFourCone", "19-0")); // 20 Bring upper arm up + addState(new TopArm(robot, "RightFourCone", "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")); // 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")); // 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")); - // 24 Drive Back and lift up all the way to position for the low junction (parallel state) + // 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")); // 25 Drive back faster after the cone is fully off of the stack + addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0")); // 26 Turn and look for the low junction with the distance sensor and align + addState(new RotationState(robot, "RightFourCone", "26-0")); // 27 Drive forward / backwards if you need to. (check with distance sensor) + addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1")); // 28 Bring Upper arm down on junction + addState(new TopArm(robot, "RightFourCone", "28-0")); // 29 Let go of cone right after arm is in position + addState(new CollectorState(robot, "RightFourCone", "29-0")); // 30 Raise arm as soon as the cone is dropped + addState(new TopArm(robot, "RightFourCone", "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")); // 32 Rotate towards Stack of cones + addState(new RotationState(robot, "RightFourCone", "32-0")); // 33 Decide which path after scanning image from earlier + addState(new PathDecision(robot, "RightFourCone", "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")); // 35 Rotate towards alliance terminal + addState(new RotationState(robot, "RightFourCone", "35-0")); } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java new file mode 100644 index 0000000..d4d7805 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java @@ -0,0 +1,112 @@ +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 new file mode 100644 index 0000000..622fe32 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java @@ -0,0 +1,112 @@ +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.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", "10-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/DriverStateWithOdometerUpperArmParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java new file mode 100644 index 0000000..1ac7bdc --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java @@ -0,0 +1,112 @@ +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 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); + } + + @Override + public void exec() { + + addParallelState(new TopArm(robot, "RightFourCone", "04-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/JunctionAllignmentState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java index 09f65c4..e2377b4 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java @@ -42,7 +42,7 @@ public class JunctionAllignmentState extends CyberarmState { break; } // the state is finished if the distance sensors are at the correct distance. - if (leftDistance == TargetSensorDistance && rightDistance == TargetSensorDistance) { + if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) { robot.frontLeftDrive.setPower(0); robot.frontRightDrive.setPower(0); robot.backLeftDrive.setPower(0); @@ -77,29 +77,29 @@ public class JunctionAllignmentState extends CyberarmState { robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); } - // The right sensor is aligned but the robot must rotate rotate CW with only the left side powered - else if (leftDistance < TargetSensorDistance && rightDistance == TargetSensorDistance) { + // The right sensor is aligned but the robot must rotate CW with only the left side powered + else 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 - else if (leftDistance > TargetSensorDistance && rightDistance == TargetSensorDistance) { + else 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 rotate CW with only the right side powered - else if (leftDistance == TargetSensorDistance && rightDistance < TargetSensorDistance) { + // The left sensor is aligned but the robot must rotate CW with only the right side powered + else 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 - else if (leftDistance == TargetSensorDistance && rightDistance > TargetSensorDistance) { + else if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) { robot.frontLeftDrive.setPower(0); robot.frontRightDrive.setPower(drivePower); robot.backLeftDrive.setPower(0); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmRotateParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmRotateParallelState.java new file mode 100644 index 0000000..c28e5dd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmRotateParallelState.java @@ -0,0 +1,82 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.TeleOp.states.PhoenixBot1; + +public class TopArmRotateParallelState extends CyberarmState { + + private final boolean stateDisabled; + PhoenixBot1 robot; + double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance; + long time; + long lastStepTime = 0; + boolean up; + boolean directPosition; + + public TopArmRotateParallelState(PhoenixBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "UpperRiserLeftPos").value(); + this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "UpperRiserRightPos").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; + + } + + @Override + public void start() { + up = robot.HighRiserLeft.getPosition() < UpperRiserLeftPos; + } + + @Override + public void exec() { + addParallelState(new RotationState(robot, "RightFourCone", "12-0")); + + if (stateDisabled){ + setHasFinished(true); + return; + } + + if (directPosition) { + robot.HighRiserLeft.setPosition(UpperRiserLeftPos); + robot.HighRiserRight.setPosition(UpperRiserRightPos); + + if (runTime() >= time){ + setHasFinished(true); + } + } else { + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + + if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) { + + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance); + + } else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) { + + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance); + + } else { + setHasFinished(true); + } + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("UpperRiserRightPos",UpperRiserRightPos); + engine.telemetry.addData("UpperRiserLeftPos",UpperRiserLeftPos); + engine.telemetry.addData("AddedDistance",AddedDistance); + engine.telemetry.addData("time",time); + engine.telemetry.addData("servo position left", robot.HighRiserLeft.getPosition()); + engine.telemetry.addData("servo position Right", robot.HighRiserRight.getPosition()); + + + + } +} 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 5b81e66..b46516b 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -19,6 +19,10 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig public class PhoenixBot1 { + private static final float mmPerInch = 25.4f; + public static final double WHEEL_CIRCUMFERENCE = 7.42108499; + public static final int COUNTS_PER_REVOLUTION = 8192; + // private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite"; private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite"; @@ -52,6 +56,14 @@ public class PhoenixBot1 { public AdafruitI2cColorSensor AdafruitEncoder; + public double ticksToInches(int ticks) { + return ticks * (WHEEL_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); + } + + public double inchesToTicks(double inches) { + return inches * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE); + } + public PhoenixBot1(CyberarmEngine engine) { this.engine = engine; @@ -62,6 +74,7 @@ public class PhoenixBot1 { } private void setupRobot () { + collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance"); leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");