From 21d14cf8789df130a5a193ab6721da91cfd38da7 Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Mon, 20 Jan 2020 18:43:51 +0100
Subject: [PATCH] new protocol for camera

---
 include/data_source_camera.h | 11 ++++--
 src/data_source_camera.cpp   | 67 +++++++++++++++++++++++++++++++-----
 2 files changed, 68 insertions(+), 10 deletions(-)

diff --git a/include/data_source_camera.h b/include/data_source_camera.h
index d4725f9..18d1c50 100755
--- a/include/data_source_camera.h
+++ b/include/data_source_camera.h
@@ -1,4 +1,7 @@
 #pragma once
+#define startp 0b01111111
+#define endp 0b10000000
+#define unkn 0b01101001 
 #include "data_source.h"
 
 class DataSourceCamera : public DataSource{
@@ -11,7 +14,11 @@ class DataSourceCamera : public DataSource{
         void readSensor() override;
         int getValueAtk(bool);
         int getValueDef(bool);
-        
+
+        int count = 0, unkn_counter;
+        byte xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy;
+        bool data_received = false, start = false, end = false;
+
         int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
         int cameraReady;
         char value;
@@ -22,5 +29,5 @@ class DataSourceCamera : public DataSource{
         int datavalid = 0;
         String valStringY = ""; 
         String valStringB = "";
-        bool negateB, negateY;
+        bool negateB, negateY; 
 };
\ No newline at end of file
diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index 8b0c3ca..82f44ee 100755
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -4,7 +4,59 @@
 DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
 
 void DataSourceCamera :: readSensor(){
-  portx = 999;
+  while(ser->available() > 0){
+    value = (byte)ser->read();
+    if(value==startp){
+      start=true;
+      count=0;
+    }
+    else if(value==endp){
+      end=true;
+      start=false;
+      data_received=false;
+      if(count==3) {
+        data_received=true;
+        xb=true_xb;
+        yb=true_yb;
+        xy=true_xy;
+        yy=true_yy;
+      }
+      }else{
+        if (count==0) xb=value;
+        else if (count==1) xy=value;
+        else if (count==2) yb=value;
+        else if (count==3) yy=value;
+        count++;
+    }   
+  }
+}
+
+
+/* 
+    for(int i=0;i<6;i++) reading[i]=(char)ser->read();
+    if(reading[0]==startp) start=true;
+      else if(reading[0]==endp && start==true) continue; 			
+	  if(reading[1]==startp && (start==true || count==0)) continue; //ser->flush();	//ignoring data and continuing
+      else if (reading[1] == unkn) unkn_counter++;//xb = oldxb;
+	  else reading[1]=xb;
+	  count++;  
+	  if(reading[2]==startp && start==true) break;
+	  else reading[2]=yb;
+	  count++;
+	  if(reading[3]==startp && start==true) break;
+      else if (reading[3] == unkn) unkn_counter++;//xy = oldxy; //tried to do for now
+	  else reading[3]=xy;
+	  count++;
+	  if(reading[4]==startp && start==true) break;
+	  else reading[4]=yy;
+	  count++;
+	  if(reading[5]==endp && count==4){
+	    end=true;
+	    data_received=true;
+	    start=false;
+	  } 
+*/
+  /* portx = 999;
   while(ser->available() > 0) {
     value = ser->read();
     // if the incoming character is a 'Y', set the start packet flag
@@ -62,10 +114,11 @@ void DataSourceCamera :: readSensor(){
     }
   }
 }
+*/
 
 void DataSourceCamera :: postProcess(){
 
-  if (valY != -74)
+  /*if (valY != -74)
     oldGoalY = valY;
   if (valB != -74)
     oldGoalB = valB;
@@ -89,7 +142,7 @@ void DataSourceCamera :: postProcess(){
 
     datavalid = 0;
     cameraReady = 1;  //attivo flag di ricezione pacchetto
-  }
+  }*/
 }
 
 int DataSourceCamera::getValueAtk(bool fixed){
@@ -115,13 +168,12 @@ int DataSourceCamera::getValueDef(bool fixed){
   if(digitalRead(SWITCH_DX) == LOW){
     if(fixed) return fixCamIMU(valB);
     return valB;
-  }
+  } 
 }
 
 void DataSourceCamera::test(){
     goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
-    
-
+   
     DEBUG.print(pAtk);
     DEBUG.print(" | ");
     DEBUG.print(fixCamIMU(pAtk));
@@ -129,8 +181,7 @@ void DataSourceCamera::test(){
 
     DEBUG.print(pDef);
     DEBUG.print(" | ");
-    DEBUG.println(fixCamIMU(pDef));
-    delay(100);
+    DEBUG.println(fixCamIMU(pDef)); 
 }
 
 int DataSourceCamera::fixCamIMU(int d){