autnomous work added

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"));
+        addState(new TopArm(robot, "TestAutonomous", "11-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
+        // Rotate to either set up for cones to grab or park somewhere
-        addState(new DriverState(robot, "TestAutonomous", "13-0"));
+        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"));
+
 
 
 

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();

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;
+}
+
+
+}

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);
 

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.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_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);

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) {

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) {
+        if (stateDisabled){
-            lastStepTime = System.currentTimeMillis();
+            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);
+            if (runTime() >= time){
-                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);
             }
+        } 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);
+                }
+            }
         }
     }
 

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;