From 7cab3a282085e29a13457dd04b99c71b50e26b04 Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Mon, 18 Nov 2019 17:15:32 +0100
Subject: [PATCH] implemented camera

---
 include/data_source_camera.h |  22 +++++++
 include/sensors.h            |   1 +
 src/data_source_camera.cpp   | 115 +++++++++++++++++++++++++++++++++++
 src/sensors.cpp              |   1 +
 4 files changed, 139 insertions(+)

diff --git a/include/data_source_camera.h b/include/data_source_camera.h
index e69de29..6ed0f02 100644
--- a/include/data_source_camera.h
+++ b/include/data_source_camera.h
@@ -0,0 +1,22 @@
+#include "data_source.h"
+
+class DataSourceCamera : public DataSource{
+
+    public:
+        DataSourceCamera(HardwareSerial* ser, int baud);
+        void postProcess() override;
+        void test() override;
+        void fixCamIMU(int);
+        
+        int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
+        int cameraready;
+        char value;
+        int startpY = 0;
+        int startpB = 0;
+        int endpY = 0; 
+        int endpB = 0;
+        int datavalid = 0;
+        String valStringY = ""; 
+        String valStringB = "";
+        bool negateB, negateY;
+};
\ No newline at end of file
diff --git a/include/sensors.h b/include/sensors.h
index 54fb6c3..1334c70 100644
--- a/include/sensors.h
+++ b/include/sensors.h
@@ -15,4 +15,5 @@ void updateSensors();
 
 extr DataSource* compass;
 extr DataSource* ball;
+extr DataSource* camera;
 extr DriveController* drive;
diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index e69de29..b6c9701 100644
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -0,0 +1,115 @@
+#include "data_source_camera.h"
+#include "sensors.h"
+
+DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
+
+void DataSourceCamera :: readSensor(){
+  portx = 999;
+  while(ser->available() > 0) value = ser->read();
+}
+
+void DataSourceCamera :: postProcess(){
+    // if the incoming character is a 'Y', set the start packet flag
+    if (inChar == 'Y') {
+      startpY = 1;
+    }
+    // if the incoming character is a 'Y', set the start packet flag
+    if (inChar == 'B') {
+      startpB = 1;
+    }
+    // if the incoming character is a '.', set the end packet flag
+    if (inChar == 'y') {
+      endpY = 1;
+    }
+    // if the incoming character is a '.', set the end packet flag
+    if (inChar == 'b') {
+      endpB = 1;
+    }
+
+    if ((startpY == 1) && (endpY == 0)) {
+      if (isDigit(inChar)) {
+        // convert the incoming byte to a char and add it to the string:
+        valStringY += inChar;
+      }else if(inChar == '-'){
+        negateY = true;
+      }
+    }
+
+    if ((startpB == 1) && (endpB == 0)) {
+      if (isDigit(inChar)) {
+        // convert the incoming byte to a char and add it to the string:
+        valStringB += inChar;
+      }else if(inChar == '-'){
+        negateB = true;
+      }
+    }
+
+    if ((startpY == 1) && (endpY == 1)) {
+      valY = valStringY.toInt(); // valid data
+      if(negateY) valY *= -1;
+      valStringY = "";
+      startpY = 0;
+      endpY = 0;
+      negateY = false;
+      datavalid ++;
+    }
+    if ((startpB == 1) && (endpB == 1)) {
+      valB = valStringB.toInt(); // valid data
+      if(negateB) valB *= -1;
+      valStringB = "";
+      startpB = 0;
+      endpB = 0;
+      negateB = false;
+      datavalid ++;
+    }
+
+  } // end of while
+
+  if (valY != -74)
+    oldGoalY = valY;
+  if (valB != -74)
+    oldGoalB = valB;
+
+  if (valY == -74)
+    valY = oldGoalY;
+  if (valB == -74)
+    valB = oldGoalB;
+  
+  // entro qui solo se ho ricevuto i pacchetti completi sia del blu che del giallo
+  if (datavalid > 1 ) {
+    if(goal_orientation == 1){
+      //yellow goalpost
+      pAtk = valY;
+      pDef = valB * -1;
+    }else{
+      //blue goalpost
+      pAtk = valB;
+      pDef = valY * -1;
+    }
+
+    datavalid = 0;
+    cameraReady = 1;  //attivo flag di ricezione pacchetto
+}
+
+void DataSourceCamera :: test(){
+    goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
+    
+
+    DEBUG_PRINT.print(pAtk);
+    DEBUG_PRINT.print(" | ");
+    DEBUG_PRINT.print(fixCamIMU(pAtk));
+    DEBUG_PRINT.print(" --- ");
+
+    DEBUG_PRINT.print(pDef);
+    DEBUG_PRINT.print(" | ");
+    DEBUG_PRINT.println(fixCamIMU(pDef));
+    delay(100);
+}
+
+void DataSourceCamera :: fixCamIMU(int d){
+    if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue();
+    else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360;
+    imuOff = constrain(imuOff*0.8, -30, 30);
+
+    return d + imuOff;
+}
\ No newline at end of file
diff --git a/src/sensors.cpp b/src/sensors.cpp
index fa0c6f8..98842ee 100644
--- a/src/sensors.cpp
+++ b/src/sensors.cpp
@@ -5,6 +5,7 @@ void initSensors(){
     compass = new DataSourceBNO055();
     drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     ball = new DataSourceBall(&Serial4, 57600);
+    camera = new DataSourceCamera(&Serial2, 19200);
 }
 
 void updateSensors(){