diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index ecc6c28..cca4e0b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.Autonomous.States.CollectorDistanceState; import org.timecrafters.Autonomous.States.CollectorState; import org.timecrafters.Autonomous.States.DriverState; import org.timecrafters.Autonomous.States.BottomArm; @@ -31,21 +32,28 @@ public class TestAutonomousEngine extends CyberarmEngine { //drive forward addState(new DriverState(robot, "TestAutonomous", "05-0")); //lower the bottom arm to get closer - addState(new BottomArm(robot, "TestAutonomous", "06-0")); + addState(new TopArm(robot, "TestAutonomous", "06-0")); //make collector release the cone addState(new CollectorState(robot, "TestAutonomous", "07-0")); //lift the lower arm to clear the pole - addState(new BottomArm(robot, "TestAutonomous", "08-0")); + addState(new TopArm(robot, "TestAutonomous", "08-0")); //Drive Backwards addState(new DriverState(robot, "TestAutonomous", "09-0")); - // Rotate to either set up for cones to grab or park somewhere - addState(new RotationState(robot, "TestAutonomous", "10-0")); // lower the bottom arm so we dont fall over - addState(new BottomArm(robot, "TestAutonomous", "11-0")); + addState(new BottomArm(robot, "TestAutonomous", "10-0")); // lower the upper arm so we dont fall over - addState(new TopArm(robot, "TestAutonomous", "12-0")); - //either dont move if your in the right zone otherwise drive forward a specific variable if we aren't already in the right spot - addState(new DriverState(robot, "TestAutonomous", "13-0")); + addState(new TopArm(robot, "TestAutonomous", "11-0"));; + // Rotate to either set up for cones to grab or park somewhere + addState(new RotationState(robot, "TestAutonomous", "12-0")); + //adjust arm height to cone. + addState(new TopArm(robot, "TestAutonomous", "13-0")); + //drive towards stack of cones + addState(new CollectorDistanceState(robot, "", "")); +// DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0"); +// addState(driverState); +// //collect next cone +// driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-0")); + diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java index 4c044f8..0865724 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -5,6 +5,7 @@ import org.timecrafters.testing.states.PrototypeBot1; public class BottomArm extends CyberarmState { + private final boolean stateDisabled; PrototypeBot1 robot; double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance; long time; @@ -18,6 +19,8 @@ public class BottomArm extends CyberarmState { this.time = robot.configuration.variable(groupName, actionName, "time").value(); this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } @Override @@ -27,6 +30,10 @@ public class BottomArm extends CyberarmState { @Override public void exec() { + if (stateDisabled){ + setHasFinished(true); + return; + } if (System.currentTimeMillis() - lastStepTime >= time) { lastStepTime = System.currentTimeMillis(); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java new file mode 100644 index 0000000..3e8abe6 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -0,0 +1,57 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.testing.states.PrototypeBot1; + +public class CollectorDistanceState extends CyberarmState { + + private final PrototypeBot1 robot; + + @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_USING_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + robot.collectorLeft.setPower(1); + robot.collectorRight.setPower(1); + } + + + + @Override + public void exec() { + if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { + robot.frontRightDrive.setPower(0.15); + robot.frontLeftDrive.setPower(0.15); + robot.backRightDrive.setPower(0.15); + robot.backLeftDrive.setPower(0.15); + } else { + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.collectorRight.setPower(0); + robot.collectorLeft.setPower(0); + setHasFinished(true); + + } + + } + public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; +} + + +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java index e94a68e..ff89164 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java @@ -1,11 +1,13 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.testing.states.PrototypeBot1; public class CollectorState extends CyberarmState { private final PrototypeBot1 robot; + private final boolean stateDisabled; private boolean collecting; private long duration; private long BeginningofActionTime; @@ -16,6 +18,9 @@ public class CollectorState extends CyberarmState { this.duration = robot.configuration.variable(groupName, actionName, "Duration").value(); this.collecting = robot.configuration.variable(groupName, actionName, "Collecting").value(); + + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } @@ -23,6 +28,7 @@ public class CollectorState extends CyberarmState { public void telemetry() { engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition()); engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition()); + engine.telemetry.addData("Collector Distance", robot.collectorDistance.getDistance(DistanceUnit.MM)); } @Override @@ -32,12 +38,16 @@ public class CollectorState extends CyberarmState { @Override public void exec() { + if (stateDisabled){ + setHasFinished(true); + return; + } if (System.currentTimeMillis() - BeginningofActionTime < duration) { if (collecting) { robot.collectorRight.setPower(1); robot.collectorLeft.setPower(1); - } else { + } else if (collecting != true){ robot.collectorRight.setPower(-1); robot.collectorLeft.setPower(-1); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index 4d997b8..22ebba6 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -6,12 +6,14 @@ import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.testing.states.PrototypeBot1; public class DriverState extends CyberarmState { + private final boolean stateDisabled; PrototypeBot1 robot; public DriverState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } private double drivePower; private int traveledDistance; @@ -23,14 +25,18 @@ public class DriverState extends CyberarmState { robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_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() { + if (stateDisabled) { + setHasFinished(true); + return; + } if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){ robot.backLeftDrive.setPower(drivePower); 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 c3e4dac..4cd2124 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -4,11 +4,14 @@ import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.testing.states.PrototypeBot1; public class RotationState extends CyberarmState { + private final boolean stateDisabled; PrototypeBot1 robot; public RotationState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } @@ -18,7 +21,10 @@ public class RotationState extends CyberarmState { @Override public void exec() { - + if (stateDisabled){ + setHasFinished(true); + return; + } RobotRotation = robot.imu.getAngularOrientation().firstAngle; if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) { 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 baaf08e..eceb1b3 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java @@ -5,11 +5,13 @@ import org.timecrafters.testing.states.PrototypeBot1; public class TopArm extends CyberarmState { + private final boolean stateDisabled; PrototypeBot1 robot; double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance; long time; long lastStepTime = 0; boolean up; + boolean directPosition; public TopArm(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -17,6 +19,10 @@ public class TopArm extends CyberarmState { 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 @@ -26,22 +32,36 @@ public class TopArm extends CyberarmState { @Override public void exec() { - if (System.currentTimeMillis() - lastStepTime >= time) { - lastStepTime = System.currentTimeMillis(); + if (stateDisabled){ + setHasFinished(true); + return; + } - if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) { + if (directPosition) { + robot.HighRiserLeft.setPosition(UpperRiserLeftPos); + robot.HighRiserRight.setPosition(UpperRiserRightPos); - 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 { + 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); + } + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 8c8d9a2..4b7a5e1 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -1,6 +1,7 @@ package org.timecrafters.testing.states; import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -35,6 +36,8 @@ public class PrototypeBot1 { public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight; private final CyberarmEngine engine; + public Rev2mDistanceSensor collectorDistance; + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive; public CRServo collectorLeft, collectorRight; @@ -53,6 +56,8 @@ public class PrototypeBot1 { } private void setupRobot () { + collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); parameters.mode = BNO055IMU.SensorMode.IMU;