Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java

@@ -5,6 +5,8 @@ import android.util.Log;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
+import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.concurrent.CopyOnWriteArrayList;
 
 /**
@@ -17,6 +19,7 @@ public abstract class CyberarmEngine extends OpMode {
   public static CyberarmEngine instance;
   //Array To Hold States
   final private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
+    public HashMap<String, String> blackboard = new HashMap<>();
     private int activeStateIndex = 0;
   private boolean isRunning;
 

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

@@ -0,0 +1,96 @@
+package org.timecrafters.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Autonomous.States.BottomArm;
+import org.timecrafters.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Autonomous.States.CollectorState;
+import org.timecrafters.Autonomous.States.ConeIdentification;
+import org.timecrafters.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Autonomous.States.DriverState;
+import org.timecrafters.Autonomous.States.PathDecision;
+import org.timecrafters.Autonomous.States.RotationState;
+import org.timecrafters.Autonomous.States.TopArm;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+@Autonomous (name = "Left Side")
+
+public class LeftSideAutonomousEngine extends CyberarmEngine {
+    PrototypeBot1 robot;
+
+    @Override
+    public void setup() {
+        robot = new PrototypeBot1(this);
+        addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0"));
+        //drive to high pole
+        addState(new DriverState(robot, "LeftSideAutonomous", "01-0"));
+        //turn towards high pole
+        addState(new RotationState(robot, "LeftSideAutonomous", "02-0"));
+        //lift the upper arm
+        addState(new TopArm(robot, "LeftSideAutonomous", "03-0"));
+        //lift the lower arm
+        addState(new BottomArm(robot, "LeftSideAutonomous", "04-0"));
+        //drive forward to junction
+        addState(new DriverState(robot, "LeftSideAutonomous", "05-0"));
+        //lower the bottom arm to get closer
+        addState(new TopArm(robot, "LeftSideAutonomous", "06-0"));
+        //make collector release the cone
+        addState(new CollectorState(robot, "LeftSideAutonomous", "07-0"));
+        //lift the lower arm to clear the pole
+        addState(new TopArm(robot, "LeftSideAutonomous", "08-0"));
+        //Drive Backwards away from junction
+        addState(new DriverState(robot, "LeftSideAutonomous", "09-0"));
+        // Rotate to either set up for cones to grab or park somewhere
+        addState(new RotationState(robot, "LeftSideAutonomous", "10-0"));
+        // lower the bottom arm so we dont fall over
+        addState(new BottomArm(robot, "LeftSideAutonomous", "11-0"));
+        // lower the upper arm so we dont fall over
+        addState(new TopArm(robot, "LeftSideAutonomous", "12-0"));;
+        //adjust arm height to cone.
+        addState(new TopArm(robot, "LeftSideAutonomous", "13-0"));
+        //drive towards stack of cones while collecting
+        addState(new CollectorDistanceState(robot, "LeftSideAutonomous", "14-0"));
+        //drive slightly back
+        addState(new DriverState(robot, "LeftSideAutonomous", "15-0"));
+        // face to -90
+        addState(new RotationState(robot, "LeftSideAutonomous", "15-1"));
+        //lift arm up
+        addState(new TopArm(robot, "LeftSideAutonomous", "16-0"));
+        // drive backwards too position
+        addState(new DriverState(robot, "LeftSideAutonomous", "17-0"));
+        // face low junction
+        addState(new RotationState(robot, "LeftSideAutonomous", "18-0"));
+        // drive up to junction
+        addState(new DriverState(robot, "LeftSideAutonomous", "19-0"));
+        // bring arm down.
+        addState(new TopArm(robot, "LeftSideAutonomous", "20-0"));
+        // get rid of cone
+        addState(new CollectorState(robot, "LeftSideAutonomous", "21-0"));
+        // lift arm up to clear
+        addState(new TopArm(robot, "LeftSideAutonomous", "22-0"));
+        // rotate towards stack of cones
+        addState(new RotationState(robot, "LeftSideAutonomous", "23-0"));
+        // Choose Parking Spot
+        addState(new PathDecision(robot, "LeftSideAutonomous", "24-0"));
+        // case 1 drive forward
+        addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-1"));
+        // case 2 drive forward
+        addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-2"));
+        // case 3 drive forward
+        addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-3"));
+        // zero out
+        addState(new RotationState(robot, "LeftSideAutonomous", "25-0"));
+        // Top Arm Down
+        addState(new TopArm(robot, "LeftSideAutonomous", "25-1"));
+        //Drive Backwards
+        addState(new DriverState(robot, "LeftSideAutonomous", "26-0"));
+    }
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
+    }
+}

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

@@ -0,0 +1,98 @@
+package org.timecrafters.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Autonomous.States.CollectorDistanceState;
+import org.timecrafters.Autonomous.States.CollectorState;
+import org.timecrafters.Autonomous.States.ConeIdentification;
+import org.timecrafters.Autonomous.States.DriverParkPlaceState;
+import org.timecrafters.Autonomous.States.DriverState;
+import org.timecrafters.Autonomous.States.BottomArm;
+import org.timecrafters.Autonomous.States.PathDecision;
+import org.timecrafters.Autonomous.States.RotationState;
+import org.timecrafters.Autonomous.States.TopArm;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+@Autonomous (name = "Right Side")
+
+public class RightSideAutonomousEngine extends CyberarmEngine {
+    PrototypeBot1 robot;
+
+    @Override
+    public void setup() {
+        robot = new PrototypeBot1(this);
+        addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0"));
+        //drive to high pole
+        addState(new DriverState(robot, "RightSideAutonomous", "01-0"));
+        //turn towards high pole
+        addState(new RotationState(robot, "RightSideAutonomous", "02-0"));
+        //lift the upper arm
+        addState(new TopArm(robot, "RightSideAutonomous", "03-0"));
+        //lift the lower arm
+        addState(new BottomArm(robot, "RightSideAutonomous", "04-0"));
+        //drive forward
+        addState(new DriverState(robot, "RightSideAutonomous", "05-0"));
+        // drive backward
+        addState(new DriverState(robot, "RightSideAutonomous", "05-1"));
+        //lower the bottom arm to get closer
+        addState(new TopArm(robot, "RightSideAutonomous", "06-0"));
+        //make collector release the cone
+        addState(new CollectorState(robot, "RightSideAutonomous", "07-0"));
+        //lift the lower arm to clear the pole
+        addState(new TopArm(robot, "RightSideAutonomous", "08-0"));
+        //Drive Backwards
+        addState(new DriverState(robot, "RightSideAutonomous", "09-0"));
+        // lower the bottom arm so we dont fall over
+        addState(new BottomArm(robot, "RightSideAutonomous", "10-0"));
+        // lower the upper arm so we dont fall over
+        addState(new TopArm(robot, "RightSideAutonomous", "11-0"));;
+        // Rotate to either set up for cones to grab or park somewhere
+        addState(new RotationState(robot, "RightSideAutonomous", "12-0"));
+        //adjust arm height to cone.
+        addState(new TopArm(robot, "RightSideAutonomous", "13-0"));
+        //drive towards stack of cones while collecting
+        addState(new CollectorDistanceState(robot, "RightSideAutonomous", "14-0"));
+        //drive slightly back
+        addState(new DriverState(robot, "RightSideAutonomous", "15-0"));
+        // face to -90
+        addState(new RotationState(robot, "RightSideAutonomous", "15-1"));
+        //lift arm up
+        addState(new TopArm(robot, "RightSideAutonomous", "16-0"));
+        // drive backwards too position
+        addState(new DriverState(robot, "RightSideAutonomous", "17-0"));
+        // rotate
+        addState(new RotationState(robot, "RightSideAutonomous", "18-0"));
+        // drive slightly towards junction
+        addState(new DriverState(robot, "RightSideAutonomous", "18-1"));
+        // drive away from junction
+        addState(new DriverState(robot, "RightSideAutonomous", "18-2"));
+        // bring arm down.
+        addState(new TopArm(robot, "RightSideAutonomous", "19-0"));
+        // get rid of cone
+        addState(new CollectorState(robot, "RightSideAutonomous", "20-0"));
+        // lift arm up to clear
+        addState(new TopArm(robot, "RightSideAutonomous", "21-0"));
+        // rotate towards stack of cones
+        addState(new RotationState(robot, "RightSideAutonomous", "22-0"));
+        // Choose Parking Spot
+        addState(new PathDecision(robot, "RightSideAutonomous", "23-0"));
+        // case 1 drive forward
+        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-1"));
+        // case 2 drive forward
+        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-2"));
+        // case 3 drive forward
+        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-3"));
+        // zero out
+        addState(new RotationState(robot, "RightSideAutonomous", "24-0"));
+        // Top Arm Down
+        addState(new TopArm(robot, "RightSideAutonomous", "25-0"));
+    }
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
+    }
+}

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

@@ -1,66 +0,0 @@
-package org.timecrafters.Autonomous.Engines;
-
-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;
-import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.testing.states.PrototypeBot1;
-
-@Autonomous (name = "Autonomous Test")
-
-public class TestAutonomousEngine extends CyberarmEngine {
-    PrototypeBot1 robot;
-
-    @Override
-    public void setup() {
-        robot = new PrototypeBot1(this);
-
-        //drive to high pole
-        addState(new DriverState(robot, "TestAutonomous", "01-0"));
-        //turn towards high pole
-        addState(new RotationState(robot, "TestAutonomous", "02-0"));
-        //lift the upper arm
-        addState(new TopArm(robot, "TestAutonomous", "03-0"));
-        //lift the lower arm
-        addState(new BottomArm(robot, "TestAutonomous", "04-0"));
-        //drive forward
-        addState(new DriverState(robot, "TestAutonomous", "05-0"));
-        //lower the bottom arm to get closer
-        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 TopArm(robot, "TestAutonomous", "08-0"));
-        //Drive Backwards
-        addState(new DriverState(robot, "TestAutonomous", "09-0"));
-        // lower the bottom arm so we dont fall over
-        addState(new BottomArm(robot, "TestAutonomous", "10-0"));
-        // lower the upper arm so we dont fall over
-        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"));
-
-
-
-
-
-
-
-
-
-    }
-}

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

@@ -11,6 +11,7 @@ public class BottomArm extends CyberarmState {
     long time;
     long lastStepTime = 0;
     boolean up;
+    boolean directPosition;
 
     public BottomArm(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -18,6 +19,7 @@ public class BottomArm extends CyberarmState {
         this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").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;
 
@@ -35,6 +37,14 @@ public class BottomArm extends CyberarmState {
             return;
         }
 
+        if (directPosition) {
+            robot.LowRiserLeft.setPosition(LowerRiserLeftPos);
+            robot.LowRiserRight.setPosition(LowerRiserLeftPos);
+
+            if (runTime() >= time){
+                setHasFinished(true);
+            }
+        } else {
             if (System.currentTimeMillis() - lastStepTime >= time) {
                 lastStepTime = System.currentTimeMillis();
 
@@ -53,6 +63,7 @@ public class BottomArm extends CyberarmState {
                 }
             }
         }
+    }
 
     @Override
     public void telemetry() {

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

@@ -9,6 +9,56 @@ import org.timecrafters.testing.states.PrototypeBot1;
 public class CollectorDistanceState extends CyberarmState {
 
     private final PrototypeBot1 robot;
+    private int traveledDistance;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private double drivePower;
+    private double targetDrivePower;
+    private double lastMeasuredTime;
+    private double currentDistance;
+    private double LastDistanceRead;
+    private double distanceDelta;
+    private double debugRunTime;
+    private String debugStatus = "?";
+
+
+    public CollectorDistanceState(PrototypeBot1 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();
+
+    }
+
+    @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.addLine();
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("Current Distance", currentDistance);
+        engine.telemetry.addData("last Distance", LastDistanceRead);
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("distanceDelta", distanceDelta);
+        engine.telemetry.addData("DEBUG RunTime", debugRunTime);
+        engine.telemetry.addData("DEBUG Status", debugStatus);
+
+    }
 
     @Override
     public void start() {
@@ -26,17 +76,72 @@ public class CollectorDistanceState extends CyberarmState {
 
         robot.collectorLeft.setPower(1);
         robot.collectorRight.setPower(1);
+
+        lastMeasuredTime = System.currentTimeMillis();
+        LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM);
+
+
+
     }
 
 
 
     @Override
     public void exec() {
+
+        if (System.currentTimeMillis() - lastMeasuredTime > 150) {
+            // checking to see if time is greater than 150 milliseconds
+            lastMeasuredTime = System.currentTimeMillis();
+
+            currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM);
+
+            distanceDelta = LastDistanceRead - currentDistance;
+
+            if (distanceDelta >= 0.0 || currentDistance > 500) {
+                // I am moving forward
+                // and im close too my target.
+                LastDistanceRead = currentDistance;
+                debugRunTime = runTime();
+                debugStatus = "Driving Towards Cone";
+            } else {
+                // I have stopped
+                debugStatus = "Nothing Collected";
+                robot.collectorLeft.setPower(0);
+                robot.collectorRight.setPower(0);
+                setHasFinished(true);
+                return;
+            }
+        }
+
         if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
-        robot.frontRightDrive.setPower(0.15);
+            double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
-        robot.frontLeftDrive.setPower(0.15);
+
-        robot.backRightDrive.setPower(0.15);
+            if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
-        robot.backLeftDrive.setPower(0.15);
+                // ramping up
+                drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
+            } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
+                // ramping down
+                drivePower = ((delta / RampDownDistance) + 0.15);
+            } 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(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(drivePower);
+            }
     } else {
         robot.frontRightDrive.setPower(0);
         robot.frontLeftDrive.setPower(0);
@@ -49,9 +154,6 @@ public class CollectorDistanceState extends CyberarmState {
             }
 
         }
-    public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-}
 
+    }
 
-}

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

@@ -0,0 +1,98 @@
+package org.timecrafters.Autonomous.States;
+
+import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+import java.util.List;
+
+public class ConeIdentification extends CyberarmState {
+    PrototypeBot1 robot;
+    private int time;
+    private float minimumConfidence;
+    private int ParkPlace;
+
+    public ConeIdentification(PrototypeBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        minimumConfidence = robot.configuration.variable(groupName, actionName, "Minimum Confidence").value();
+        time = robot.configuration.variable(groupName, actionName, "time").value();
+    }
+
+    @Override
+    public void init() {
+        engine.blackboard.put("parkPlace", "1");
+        robot.tfod.activate();
+    }
+
+    @Override
+    public void telemetry() {
+        if (robot.tfod != null) {
+            // getUpdatedRecognitions() will return null if no new information is available since
+            // the last time that call was made.
+            List<Recognition> updatedRecognitions = robot.tfod.getUpdatedRecognitions();
+            if (updatedRecognitions != null) {
+                engine.telemetry.addData("# Objects Detected", updatedRecognitions.size());
+
+                // step through the list of recognitions and display image position/size information for each one
+                // Note: "Image number" refers to the randomized image orientation/number
+                for (Recognition recognition : updatedRecognitions) {
+                    double col = (recognition.getLeft() + recognition.getRight()) / 2 ;
+                    double row = (recognition.getTop()  + recognition.getBottom()) / 2 ;
+                    double width  = Math.abs(recognition.getRight() - recognition.getLeft()) ;
+                    double height = Math.abs(recognition.getTop()  - recognition.getBottom()) ;
+
+                    engine.telemetry.addData("Label", recognition.getLabel());
+                    engine.telemetry.addData(""," ");
+                    engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 );
+                    engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col);
+                    engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height);
+
+                    if (recognition.getLabel().equals("2 Bulb")) {
+                        engine.telemetry.addData("2 Bulb", engine.blackboard.put("parkPlace", "2"));
+                    } else if (recognition.getLabel().equals("3 Panel")) {
+                        engine.telemetry.addData("3 Panel",engine.blackboard.put("parkPlace", "3"));
+                    } else {
+                        engine.telemetry.addData("1 Bolt", engine.blackboard.put("parkPlace", "1"));
+                    }
+                }
+            }
+        }
+    }
+
+    @Override
+    public void exec() {
+
+        if (robot.tfod != null) {
+            // getUpdatedRecognitions() will return null if no new information is available since
+            // the last time that call was made.
+            List<Recognition> updatedRecognitions = robot.tfod.getUpdatedRecognitions();
+            if (updatedRecognitions != null) {
+                // step through the list of recognitions and display image position/size information for each one
+                // Note: "Image number" refers to the randomized image orientation/number
+                float bestConfidence = 0;
+
+                for (Recognition recognition : updatedRecognitions) {
+                    if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) {
+                        bestConfidence = recognition.getConfidence();
+
+                        if (recognition.getLabel().equals("2 Bulb")) {
+                            engine.blackboard.put("parkPlace", "2");
+                        } else if (recognition.getLabel().equals("3 Panel")) {
+                            engine.blackboard.put("parkPlace", "3");
+
+                        } else {
+                            engine.blackboard.put("parkPlace", "1");
+                        }
+                    }
+                }
+            }
+        }
+
+        if (runTime() >= time){
+            setHasFinished(true);
+        }
+
+    }
+}

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

@@ -0,0 +1,108 @@
+package org.timecrafters.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class DriverParkPlaceState extends CyberarmState {
+    private final boolean stateDisabled;
+    PrototypeBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private String intendedPlacement;
+
+    public DriverParkPlaceState(PrototypeBot1 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.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").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() {
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+        String placement = engine.blackboard.get("parkPlace");
+        if (placement != null) {
+            if (!placement.equals(intendedPlacement)){
+                    setHasFinished(true);
+            }
+            if (placement.equals(intendedPlacement)) {
+                double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
+
+                if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
+                    // ramping up
+                    drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
+                } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= 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(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
+                    robot.backLeftDrive.setPower(drivePower);
+                    robot.backRightDrive.setPower(drivePower);
+                    robot.frontLeftDrive.setPower(drivePower);
+                    robot.frontRightDrive.setPower(drivePower);
+                } else {
+                    robot.backLeftDrive.setPower(0);
+                    robot.backRightDrive.setPower(0);
+                    robot.frontLeftDrive.setPower(0);
+                    robot.frontRightDrive.setPower(0);
+                    setHasFinished(true);
+                } // else ending
+            } // placement equals if statement
+//            setHasFinished(true);
+        } // end of placement doesn't equal null
+
+    } // end of exec
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Position", intendedPlacement);
+        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("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+    } // end of telemetry
+} // end of class

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

@@ -8,14 +8,18 @@ import org.timecrafters.testing.states.PrototypeBot1;
 public class DriverState extends CyberarmState {
     private final boolean stateDisabled;
     PrototypeBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
     public DriverState(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
-        this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
+        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.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
     }
-    private double drivePower;
+    private double drivePower, targetDrivePower;
     private int traveledDistance;
 
     @Override
@@ -38,6 +42,29 @@ public class DriverState extends CyberarmState {
             return;
         }
 
+        double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
+
+        if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
+            // ramping up
+           drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
+        }
+        else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= 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(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
             robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(drivePower);
@@ -51,5 +78,22 @@ public class DriverState extends CyberarmState {
             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("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
     }
 }

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

@@ -0,0 +1,29 @@
+package org.timecrafters.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class PathDecision extends CyberarmState {
+    PrototypeBot1 robot;
+    private String groupName;
+
+
+    public PathDecision (PrototypeBot1 robot, String groupName, String actionName){
+        this.robot = robot;
+        this.groupName = groupName;
+
+    }
+
+    @Override
+    public void exec() {
+        String placement = engine.blackboard.get("parkPlace");
+        setHasFinished(true);
+        }
+
+    }
+
+
+
+
+
+

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

@@ -8,8 +8,9 @@ public class RotationState extends CyberarmState {
     PrototypeBot1 robot;
     public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
-        this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
+        this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
         this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
+        drivePowerVariable = drivePower;
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
 
 
@@ -18,20 +19,41 @@ public class RotationState extends CyberarmState {
     private double drivePower;
     private float targetRotation;
     float RobotRotation;
+    private double RotationTarget;
+    private double RotationDirectionMinimum;
+    private String debugStatus = "?";
+    private double drivePowerVariable;
+
 
     @Override
     public void exec() {
         if (stateDisabled){
             setHasFinished(true);
             return;
-        }
+            } // end of if
 
         RobotRotation = robot.imu.getAngularOrientation().firstAngle;
-        if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {
+
-            robot.backLeftDrive.setPower(-drivePower);
+        if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
-            robot.backRightDrive.setPower(drivePower);
+            drivePowerVariable = 0.3 * drivePower;
-            robot.frontLeftDrive.setPower(-drivePower);
+            debugStatus = "Rotate Slow";
-            robot.frontRightDrive.setPower(drivePower);
+        } // end of if
+        else {
+            drivePowerVariable = drivePower * 0.75;
+            debugStatus = "Rotate Fast";
+        }
+
+        if (RobotRotation >= targetRotation + 1){
+            drivePowerVariable = Math.abs(drivePowerVariable);
+        } else {
+            drivePowerVariable = -1 * Math.abs(drivePowerVariable);
+        }
+
+        if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) {
+            robot.backLeftDrive.setPower(drivePowerVariable);
+            robot.backRightDrive.setPower(-drivePowerVariable);
+            robot.frontLeftDrive.setPower(drivePowerVariable);
+            robot.frontRightDrive.setPower(-drivePowerVariable);
         } else
         {
             robot.backLeftDrive.setPower(0);
@@ -45,9 +67,13 @@ public class RotationState extends CyberarmState {
 
     @Override
     public void telemetry() {
+        engine.telemetry.addData("DEBUG Status", debugStatus);
+
+        engine.telemetry.addLine();
+
         engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
         engine.telemetry.addData("Robot Target Rotation", targetRotation);
-        engine.telemetry.addData("Drive Power", drivePower);
+        engine.telemetry.addData("Drive Power", drivePowerVariable);
 
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java

@@ -71,7 +71,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
         }
 
         /* ............................................................................ grab */
-        if(engine.gamepad1.left_stick_x>0.5 || engine.gamepad1.right_stick_x<-0.5 ||
+        if(engine.gamepad1.left_stick_x < -0.5 || engine.gamepad1.right_stick_x < -0.5 ||
                 engine.gamepad2.x){ // in
             robot.pServoGrab.setPosition(0.9);
         }
@@ -79,7 +79,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
                 engine.gamepad2.back){ // small out
             robot.pServoGrab.setPosition(0.50);
         }
-        if(engine.gamepad1.right_stick_x>0.5 || engine.gamepad1.right_stick_x>0.5 ||
+        if(engine.gamepad1.left_stick_x > 0.5 || engine.gamepad1.right_stick_x > 0.5 ||
                 engine.gamepad2.b){ // big out
             robot.pServoGrab.setPosition(0.0);
         }

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java

@@ -29,11 +29,11 @@ public class PrototypeBot1 {
     private static final String VUFORIA_KEY =
             "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
 
-    private VuforiaLocalizer vuforia;
+    public VuforiaLocalizer vuforia;
 
-    private TFObjectDetector tfod;
+    public TFObjectDetector tfod;
 
-    public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight;
+    public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo;
     private final CyberarmEngine engine;
 
     public Rev2mDistanceSensor collectorDistance;
@@ -80,6 +80,9 @@ public class PrototypeBot1 {
 
             // servo configuration
 
+            //Camera Servo
+            CameraServo = engine.hardwareMap.servo.get("Camera Servo");
+
             // Collector
             collectorLeft = engine.hardwareMap.crservo.get("Collector Left");
             collectorRight = engine.hardwareMap.crservo.get("Collector Right");
@@ -117,22 +120,26 @@ public class PrototypeBot1 {
             LowRiserLeft.setDirection(Servo.Direction.FORWARD);
             LowRiserRight.setDirection(Servo.Direction.REVERSE);
 
-            LowRiserLeft.setPosition(0.45);
+            LowRiserLeft.setPosition(0.35);
-            LowRiserRight.setPosition(0.45);
+            LowRiserRight.setPosition(0.35);
             HighRiserLeft.setPosition(0.45);
             HighRiserRight.setPosition(0.45);
 
+            CameraServo.setPosition(0);
+
         }
 
             private void initVuforia(){
                 /*
                  * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
                  */
-                VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+                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);
             }

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java

@@ -290,7 +290,7 @@ public class PrototypeTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad2.dpad_up) {
-            if (robot.HighRiserLeft.getPosition() < 0.9) {
+            if (robot.HighRiserLeft.getPosition() < 1.0) {
                 if (System.currentTimeMillis() - lastStepTime >= 150) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
@@ -310,7 +310,7 @@ public class PrototypeTeleOPState extends CyberarmState {
         }
 
         if (engine.gamepad2.y) {
-            if (robot.HighRiserLeft.getPosition() < 0.84) {
+            if (robot.HighRiserLeft.getPosition() < 0.9) {
                 if (System.currentTimeMillis() - lastStepTime >= 150) {
                     lastStepTime = System.currentTimeMillis();
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);