autnomous work added

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,7 +19,8 @@ public abstract class CyberarmEngine extends OpMode {
   public static CyberarmEngine instance;
   //Array To Hold States
   final private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
-  private int activeStateIndex = 0;
+    public HashMap<String, String> blackboard = new HashMap<>();
+    private int activeStateIndex = 0;
   private boolean isRunning;
 
   private final static String TAG = "PROGRAM.ENGINE";

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

@@ -6,6 +6,7 @@ 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.ConeIdentification;
 import org.timecrafters.Autonomous.States.DriverState;
 import org.timecrafters.Autonomous.States.BottomArm;
 import org.timecrafters.Autonomous.States.RotationState;
@@ -20,7 +21,7 @@ public class TestAutonomousEngine extends CyberarmEngine {
     @Override
     public void setup() {
         robot = new PrototypeBot1(this);
-
+        addState(new ConeIdentification(robot, "TestAutonomous", "00-0"));
         //drive to high pole
         addState(new DriverState(robot, "TestAutonomous", "01-0"));
         //turn towards high pole
@@ -67,15 +68,22 @@ public class TestAutonomousEngine extends CyberarmEngine {
         addState(new TopArm(robot, "TestAutonomous", "22-0"));
         // get rid of cone
         addState(new CollectorState(robot, "TestAutonomous", "23-0"));
+        // lift arm up to clear
+        addState(new TopArm(robot, "TestAutonomous", "24-0"));
+        // drive back
+        addState(new DriverState(robot, "TestAutonomous", "25-0"));
+        // bring bottom arm down
+        addState(new BottomArm(robot, "TestAutonomous", "26-0"));
+        // bring top arm down
+        addState(new TopArm(robot, "TestAutonomous", "27-0"));
+        // rotate towards stack of cones
+        addState(new RotationState(robot, "TestAutonomous", "28-0"));
+    }
 
+    @Override
+    public void loop() {
+        super.loop();
 
-
+        telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
-
-
-
-
-
-
-
     }
 }

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,22 +37,31 @@ public class BottomArm extends CyberarmState {
             return;
         }
 
-        if (System.currentTimeMillis() - lastStepTime >= time) {
+        if (directPosition) {
-            lastStepTime = System.currentTimeMillis();
+            robot.LowRiserLeft.setPosition(LowerRiserLeftPos);
+            robot.LowRiserRight.setPosition(LowerRiserLeftPos);
 
-            if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) {
+            if (runTime() >= time){
-
-                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance);
-                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance);
-
-            } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) {
-
-                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
-                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
-
-            } else {
                 setHasFinished(true);
             }
+        } else {
+            if (System.currentTimeMillis() - lastStepTime >= time) {
+                lastStepTime = System.currentTimeMillis();
+
+                if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) {
+
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance);
+
+                } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) {
+
+                    robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
+                    robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
+
+                } else {
+                    setHasFinished(true);
+                }
+            }
         }
     }
 

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

@@ -14,6 +14,10 @@ public class CollectorDistanceState extends CyberarmState {
     private int RampDownDistance;
     private double drivePower;
     private double targetDrivePower;
+    private double lastMeasuredTime;
+    private double currentDistance;
+    private double LastDistanceRead;
+
 
     public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -38,6 +42,8 @@ public class CollectorDistanceState extends CyberarmState {
         engine.telemetry.addData("traveledDistance", traveledDistance);
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
+        engine.telemetry.addData("Current Distance", currentDistance);
+        engine.telemetry.addData("last Distance", LastDistanceRead);
 
     }
 
@@ -57,12 +63,37 @@ 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);
+
+            if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) {
+                // I am moving forward
+                // and im close too my target.
+                LastDistanceRead = currentDistance;
+
+            } else {
+                // I have stopped
+                targetDrivePower = 0;
+//                setHasFinished(true);
+            }
+        }
+
     if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
         double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
 

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/PathDecision.java

@@ -0,0 +1,39 @@
+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");
+
+        if (placement != null) {
+            if (placement.equals("2")) {
+                engine.insertState(this, new DriverState(robot, groupName, "29-1"));
+            } else if (placement.equals("3")) {
+                engine.insertState(this, new DriverState(robot, groupName, "29-2"));
+            }
+        }
+          else {
+              engine.insertState(this, new DriverState(robot, groupName, "29-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");
@@ -122,17 +125,21 @@ public class PrototypeBot1 {
             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);
             }