From b07ed6ddd8f1f08865435fa066489bd43e53580a Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Thu, 8 Dec 2022 20:28:40 -0600
Subject: [PATCH] Vision in autonomous is added to init

---
 .../Autonomous/States/ConeIdentification.java | 28 ++++++++++++++-----
 .../TeleOp/states/PhoenixBot1.java            | 11 +++++---
 2 files changed, 28 insertions(+), 11 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java
index c606173..29261be 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java
@@ -11,6 +11,7 @@ public class ConeIdentification extends CyberarmState {
     private int time;
     private float minimumConfidence;
     private int ParkPlace;
+    long initTime;
 
     public ConeIdentification(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -22,6 +23,7 @@ public class ConeIdentification extends CyberarmState {
     public void init() {
         engine.blackboard.put("parkPlace", "1");
         robot.tfod.activate();
+        initTime = System.currentTimeMillis();
     }
 
     @Override
@@ -47,21 +49,33 @@ public class ConeIdentification extends CyberarmState {
                     engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col);
                     engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height);
 
-                    if (recognition.getLabel().equals("Red 2")) {
-                        engine.telemetry.addData("Red 2", engine.blackboard.put("parkPlace", "2"));
-                    } else if (recognition.getLabel().equals("Blue 3")) {
-                        engine.telemetry.addData("Blue 3",engine.blackboard.put("parkPlace", "3"));
+                    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("Yellow 1", engine.blackboard.put("parkPlace", "1"));
+                        engine.telemetry.addData("1 Bolt", engine.blackboard.put("parkPlace", "1"));
                     }
                 }
             }
         }
     }
 
+    @Override
+    public void start() {
+        long startTime = System.currentTimeMillis() - initTime;
+        if (startTime < time){
+            time = (int) (time - startTime);
+        }
+        else {
+            time = 0;
+        }
+    }
+
     @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.
@@ -75,9 +89,9 @@ public class ConeIdentification extends CyberarmState {
                     if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) {
                         bestConfidence = recognition.getConfidence();
 
-                        if (recognition.getLabel().equals("Red 2")) {
+                        if (recognition.getLabel().equals("2 Bulb")) {
                             engine.blackboard.put("parkPlace", "2");
-                        } else if (recognition.getLabel().equals("Blue 3")) {
+                        } else if (recognition.getLabel().equals("3 Panel")) {
                             engine.blackboard.put("parkPlace", "3");
 
                         } else {
diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
index e58c87d..e069613 100644
--- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
+++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
@@ -19,12 +19,15 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
 
 public class PhoenixBot1 {
 
-    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
+//    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
+    private static final String TFOD_MODEL_ASSET = "PowerPlay.tflite";
+
+
 
     private static final String[] LABELS = {
-            "Yellow 1",
-            "Red 2",
-            "Blue 3"
+            "1 Bolt",
+            "2 Bulb",
+            "3 Panel"
     };
 
     private static final String VUFORIA_KEY =