From d534c5b4a8c2064cb43c42f9269b7f0256894ef8 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Wed, 22 Jan 2020 17:22:32 +0100
Subject: [PATCH] working camera->teensy transimission protocol

---
 src/data_source_camera.cpp  | 17 +++++------------
 src/main.cpp                | 13 +++++++------
 utility/OpenMV/conic_eff.py |  9 +++++----
 3 files changed, 17 insertions(+), 22 deletions(-)

diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index 9a56d83..f8b4568 100755
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -12,21 +12,21 @@ void DataSourceCamera :: readSensor(){
       count=0;
     }
     else if(value==endp){
-      end=true;
-      start=false;
       data_received=false;
-      if(count==3 && start==true) {
+      if(count==4 && start==true) {
         data_received=true;
         true_xb = xb;
         true_yb = yb;
         true_xy = xy;
         true_yy = yy;
       }
+      end=true;
+      start=false;
     }else{
       if(start==true){
         if (count==0) xb=value;
-        else if (count==1) xy=value;
-        else if (count==2) yb=value;
+        else if (count==1) yb=value;
+        else if (count==2) xy=value;
         else if (count==3) yy=value;
         count++;
       }
@@ -186,13 +186,6 @@ void DataSourceCamera::test(){
     DEBUG.print(" | ");
     DEBUG.println(fixCamIMU(pDef));  */
     //update();
-    DEBUG.print(xb);
-    DEBUG.print("|");
-    DEBUG.print(yb);
-    DEBUG.print("|");
-    DEBUG.print(xy);
-    DEBUG.print("|");
-    DEBUG.print(yy); 
     DEBUG.println("---------------");
     DEBUG.print(true_xb);
     DEBUG.print("|");
diff --git a/src/main.cpp b/src/main.cpp
index 7e62f00..7852e24 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,7 +6,7 @@
 void setup() {
   delay(500);
 
-  DEBUG.begin(9600);
+  DEBUG.begin(19600);
   initSensors();
   initGames();
   
@@ -16,11 +16,12 @@ void setup() {
 
 void loop() {
   updateSensors();
-  camera->test();
-  /* 
-  goalie->play(role==1);
-  keeper->play(role==0); */
+  
+  // goalie->play(role==1);
+  // keeper->play(role==0);
 
   // Last thing to do: movement  
-/*   drive->drivePrepared();  */
+  // drive->drivePrepared(); 
+  //Serial.print("ao");
+  camera->test();
 } 
diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py
index 1a5a593..a06c5f6 100644
--- a/utility/OpenMV/conic_eff.py
+++ b/utility/OpenMV/conic_eff.py
@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (69, 100, -12, 11, 15, 89),    # thresholds yellow goal
-                (20, 51, -23, 26, -69, -31)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+thresholds = [  (60, 75, -19, 9, 9, 60),    # thresholds yellow goal
+                (16, 35, -8, 24, -41, -13)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -64,7 +64,7 @@ sensor.set_contrast(+0)
 sensor.set_saturation(+0)
 sensor.set_brightness(0)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 8000)
+sensor.set_auto_exposure(False, 3500)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
@@ -125,7 +125,7 @@ while(True):
 
     if b_found == True:
         b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 0, 100)
-        b_cx = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
+        b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
 
         #Prepare for send as a list of characters
         s_bcx = chr(b_cx)
@@ -139,6 +139,7 @@ while(True):
 
     #print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
 
+
     uart.write(START_BYTE)
     uart.write(s_ycx)
     uart.write(s_ycy)