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..4d8555a 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 - - // 10 Back up and bring lower arm down (parallel state) - - // 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor) - - // 12 Rotate towards stack (this might be parallel with last step) - - // 13 Drive at stack while collecting and check to see when we grab it - - // 14 Back up and raise arm (in parallel state) - - // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state) - - // 16 Rotate and use sensor to find junction - - // 17 Drive Towards Junction (This is optional, idk if this is needed atm) - - // 18 Bring upper arm down - - // 19 Drop cone - - // 20 Bring upper arm up - - // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier) - - // 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) - - // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack - - // 24 Drive Back and lift up all the way to position for the low junction (parallel state) - - // 25 Drive back faster after the cone is fully off of the stack - - // 26 Turn and look for the low junction with the distance sensor and align - - // 27 Drive forward / backwards if you need to. (check with distance sensor) - - // 28 Bring Upper arm down on junction - - // 29 Let go of cone right after arm is in position - - // 30 Raise arm as soon as the cone is dropped - - // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction - - // 32 Rotate towards Stack of cones - - // 33 Decide which path after scanning image from earlier - - // 34 Drive backwards, forwards, or stay put - - // 35 Rotate towards alliance terminal +// 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 +// //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 +// 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 +// 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/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index 5a3f6bd..ca82a06 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -10,12 +10,14 @@ public class DriverStateWithOdometer extends CyberarmState { PhoenixBot1 robot; private int RampUpDistance; private int RampDownDistance; + private int maximumTolerance; public DriverStateWithOdometer(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; } @@ -66,17 +68,24 @@ public class DriverStateWithOdometer extends CyberarmState { drivePower = drivePower * -1; } - if (Math.abs(CurrentPosition) < traveledDistance){ + 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); + setHasFinished(true); } @@ -90,19 +99,6 @@ public class DriverStateWithOdometer extends CyberarmState { engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition()); - - - - - - - - - - - - - engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("targetDrivePower", targetDrivePower); 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..86a5e0b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java @@ -10,6 +10,7 @@ public class JunctionAllignmentState extends CyberarmState { private double TargetSensorDistance; private final String targetedJunction; private final double drivePower; + private int whatToDo; public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -18,6 +19,14 @@ public class JunctionAllignmentState extends CyberarmState { 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)); + + + } + @Override public void exec() { @@ -41,65 +50,101 @@ public class JunctionAllignmentState extends CyberarmState { TargetSensorDistance = 200.0; break; } + + // the state is finished if the distance sensors are at the correct distance. - if (leftDistance == TargetSensorDistance && rightDistance == TargetSensorDistance) { + 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); setHasFinished(true); } + +// 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 - else if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){ + if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){ robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); } // the robot is lined up but needs to drive backward till the robot is at the specified distance - else if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){ + 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 - else if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) { + 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 - else if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) { + 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 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 + 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) { + 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 + 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) { + 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/TopArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java index d2b71db..daa24e7 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java @@ -53,7 +53,7 @@ public class TopArm extends CyberarmState { robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance); - } else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) { + } if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) { robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance); 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");