autonomous work

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java

@@ -34,8 +34,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
 
         // 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"));
-        addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
+//        addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
-
+        addState(new TopArm(robot, "RightFourCone", "04-1"));
         // 6 Raise lower arm while slowly driving at the junction (parallel state)
         addState(new BottomArm(robot, "RightFourCone", "05-0"));
 
@@ -96,23 +96,23 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
         // 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
         addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
 //
-//        // 16 Rotate and use sensor to find junction
+        // 16 Rotate and use sensor to find junction
-//        addState(new RotationState(robot, "RightFourCone", "16-0"));
+        addState(new RotationState(robot, "RightFourCone", "16-0"));
-//
-//        // 17 Drive Towards Junction (This is optional, idk if this is needed atm)
-//        addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
 //
+        // 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"));
+        addState(new TopArm(robot, "RightFourCone", "18-0"));
 //
 //        // 19 Drop cone
-//        addState(new CollectorState(robot, "RightFourCone", "19-0"));
+        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"));
+        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"));
@@ -146,7 +146,7 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
 //        addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
 //
 //        // 32 Rotate towards Stack of cones
-//        addState(new RotationState(robot, "RightFourCone", "32-0"));
+        addState(new RotationState(robot, "RightFourCone", "32-0"));
 //
 //        // 33 Decide which path after scanning image from earlier
 //        addState(new PathDecision(robot, "RightFourCone", "33-0"));

TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java

@@ -1,5 +1,7 @@
 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.TeleOp.states.PhoenixBot1;
@@ -14,7 +16,13 @@ public class JunctionAllignmentState extends CyberarmState {
     private float targetAngle;
     private float slop;
     private double checkTime = 250;
-
+    private int traveledDistance;
+    private int loopsCurrent = 0;
+    private double minDistance;
+    private double maxDistance;
+    private String whereAmI = "init";
+    private int driveLoops;
+    private boolean finishedEnabled;
 
 
     public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -23,6 +31,11 @@ public class JunctionAllignmentState extends CyberarmState {
         this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
         this.rotationAmount = robot.configuration.variable(groupName, actionName, "rotationAmount").value();
         this.slop = robot.configuration.variable(groupName, actionName, "slop").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveled distance").value();
+        this.minDistance = robot.configuration.variable(groupName, actionName, "minDistance").value();
+        this.maxDistance = robot.configuration.variable(groupName, actionName, "maxDistance").value();
+        this.finishedEnabled = robot.configuration.variable(groupName, actionName, "finishedEnabled").value();
+
 
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
     }
@@ -38,6 +51,11 @@ public class JunctionAllignmentState extends CyberarmState {
         engine.telemetry.addData("target angle", targetAngle);
         engine.telemetry.addData("current angle", currentAngle);
         engine.telemetry.addData("drive power", drivePower);
+        engine.telemetry.addData("traveled distance", traveledDistance);
+        engine.telemetry.addData("Loops", loopsCurrent);
+        engine.telemetry.addData("where am i??", whereAmI);
+        engine.telemetry.addData("time", runTime() - checkTime);
+
 
 
 
@@ -48,6 +66,23 @@ public class JunctionAllignmentState extends CyberarmState {
     @Override
     public void start() {
 
+        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        checkTime = System.currentTimeMillis() - 250;
+
+        driveLoops = 0;
+
     }
 
     @Override
@@ -62,24 +97,24 @@ public class JunctionAllignmentState extends CyberarmState {
 
             double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
             double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
-            boolean rightInRange = rightDistance > 170 && rightDistance < 200;
+            boolean rightInRange = rightDistance > minDistance && rightDistance < maxDistance;
-            boolean leftInRange = leftDistance > 170 && leftDistance < 200;
+            boolean leftInRange = leftDistance > minDistance && leftDistance < maxDistance;
 
 
             // The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0
 
-            if (loopsTotal >= 5){
+            if (loopsCurrent >= loopsTotal){
-                setHasFinished(true);
+                setHasFinished(finishedEnabled);
-            } else if (runTime() - checkTime >= 250 ) {
+            } else if (System.currentTimeMillis() - checkTime >= 250 ) {
+
+                whereAmI = "post 250 ms";
 
                 checkTime = runTime();
-                loopsTotal = loopsTotal + 1;
 
-
+                loopsCurrent = loopsCurrent + 1;
-            } else {
 
                 if (rightInRange && leftInRange){
-                    setHasFinished(true);
+                    setHasFinished(finishedEnabled);
                 }
 
                 if (rightInRange && !leftInRange) {
@@ -95,6 +130,8 @@ public class JunctionAllignmentState extends CyberarmState {
                         robot.backLeftDrive.setPower(0);
                         robot.frontLeftDrive.setPower(0);
 
+                        setHasFinished(finishedEnabled);
+
                     } else {
 
                         if (targetAngle - slop < currentAngle) {
@@ -151,9 +188,31 @@ public class JunctionAllignmentState extends CyberarmState {
 
                 }
 
-                if (!rightInRange && !leftInRange){
+                if (!rightInRange && !leftInRange && driveLoops < 2){
+
+                    if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
+                        robot.backLeftDrive.setPower(drivePower);
+                        robot.backRightDrive.setPower(drivePower);
+                        robot.frontLeftDrive.setPower(drivePower);
+                        robot.frontRightDrive.setPower(drivePower);
+
+                        driveLoops += 1;
+                    }
+
+                    } else {
+
+                        robot.backLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.frontRightDrive.setPower(0);
+
+                        robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+                        if (driveLoops == 2) {
+                            loopsCurrent = loopsTotal;
+                        }
 
-                    setHasFinished(true);
 
                     }
                 }

TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java

@@ -73,8 +73,36 @@ public class PhoenixBot1 {
             setupRobot();
         }
 
+    private void initVuforia(){
+        /*
+         * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
+         */
+        int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+        parameters.vuforiaLicenseKey = VUFORIA_KEY;
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+
+
+        //  Instantiate the Vuforia engine
+        vuforia = ClassFactory.getInstance().createVuforia(parameters);
+    }
+
+    private void initTfod() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        tfodParameters.minResultConfidence = 0.75f;
+        tfodParameters.isModelTensorFlow2 = true;
+        tfodParameters.inputSize = 300;
+        tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
+        tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
+    }
+
         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");
@@ -170,34 +198,10 @@ public class PhoenixBot1 {
             HighRiserLeft.setPosition(0.45);
             HighRiserRight.setPosition(0.45);
 
-            CameraServo.setPosition(0.8);
+            CameraServo.setPosition(0.775);
 
         }
 
-            private void initVuforia(){
-                /*
-                 * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
-                 */
-                int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
-                VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
 
-                parameters.vuforiaLicenseKey = VUFORIA_KEY;
-                parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
-
-
-                //  Instantiate the Vuforia engine
-                vuforia = ClassFactory.getInstance().createVuforia(parameters);
-            }
-
-            private void initTfod() {
-                int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
-                        "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
-                TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
-                tfodParameters.minResultConfidence = 0.75f;
-                tfodParameters.isModelTensorFlow2 = true;
-                tfodParameters.inputSize = 300;
-                tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
-                tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
-            }
 
 }