From 13134cac99fc127900a7b2a2ff66f259bb5a26ac Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Wed, 22 Jan 2020 16:55:55 +0100
Subject: [PATCH] camera fixing and new goalie(to test)

---
 include/data_source_ball.h   |  1 +
 include/data_source_camera.h |  2 +-
 src/data_source_camera.cpp   | 35 ++++++++++++++++-----------------
 src/goalie.cpp               | 38 +++++++++++++++++++++++-------------
 4 files changed, 43 insertions(+), 33 deletions(-)

diff --git a/include/data_source_ball.h b/include/data_source_ball.h
index 6beb7be..cfb47f5 100755
--- a/include/data_source_ball.h
+++ b/include/data_source_ball.h
@@ -10,4 +10,5 @@ class DataSourceBall : public DataSource{
         
         int angle, distance;
         bool ballSeen;
+        int plusang, dir, degrees2,b;
 }; 
\ No newline at end of file
diff --git a/include/data_source_camera.h b/include/data_source_camera.h
index c2a8a5e..60cdae0 100755
--- a/include/data_source_camera.h
+++ b/include/data_source_camera.h
@@ -21,7 +21,7 @@ class DataSourceCamera : public DataSource{
 
         int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
         int cameraReady;
-        char value;
+        int value;
         int startpY = 0;
         int startpB = 0;
         int endpY = 0; 
diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index 51b2a94..9a56d83 100755
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -5,7 +5,8 @@ DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(
 
 void DataSourceCamera :: readSensor(){
   while(ser->available() > 0){
-    value = (byte)ser->read();
+    value = (int)ser->read();
+    //Serial.println(value);
     if(value==startp){
       start=true;
       count=0;
@@ -14,19 +15,21 @@ void DataSourceCamera :: readSensor(){
       end=true;
       start=false;
       data_received=false;
-      if(count==3) {
+      if(count==3 && start==true) {
         data_received=true;
-        xb=true_xb;
-        yb=true_yb;
-        xy=true_xy;
-        yy=true_yy;
+        true_xb = xb;
+        true_yb = yb;
+        true_xy = xy;
+        true_yy = yy;
       }
-      }else{
+    }else{
+      if(start==true){
         if (count==0) xb=value;
         else if (count==1) xy=value;
         else if (count==2) yb=value;
         else if (count==3) yy=value;
         count++;
+      }
     }   
   }
 }
@@ -172,8 +175,8 @@ int DataSourceCamera::getValueDef(bool fixed){
 }
 
 void DataSourceCamera::test(){
-    goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
-   
+  goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
+  update();
 /*     DEBUG.print(pAtk);
     DEBUG.print(" | ");
     DEBUG.print(fixCamIMU(pAtk));
@@ -182,28 +185,24 @@ void DataSourceCamera::test(){
     DEBUG.print(pDef);
     DEBUG.print(" | ");
     DEBUG.println(fixCamIMU(pDef));  */
-    update();
+    //update();
     DEBUG.print(xb);
     DEBUG.print("|");
-    delay(100);
     DEBUG.print(yb);
     DEBUG.print("|");
-    delay(100);
     DEBUG.print(xy);
     DEBUG.print("|");
-    delay(100);
-    DEBUG.print(yy);
+    DEBUG.print(yy); 
     DEBUG.println("---------------");
     DEBUG.print(true_xb);
     DEBUG.print("|");
-    delay(100);
     DEBUG.print(true_yb);
     DEBUG.print("|");
-    delay(100);
     DEBUG.print(true_xy);
     DEBUG.print("|");
-    delay(100);
-    DEBUG.print(true_yy);
+    DEBUG.print(true_yy); 
+    DEBUG.println("---------------");
+    delay(75);
 }
 
 int DataSourceCamera::fixCamIMU(int d){
diff --git a/src/goalie.cpp b/src/goalie.cpp
index 3849363..5ce4cf9 100755
--- a/src/goalie.cpp
+++ b/src/goalie.cpp
@@ -22,7 +22,20 @@ void Goalie::realPlay(){
 }
 
 void Goalie::goalie() {
-  if(ball->angle >= 350 || ball->angle <= 10)  {
+  if(ball->angle > 340 || ball->angle < 20) ball->plusang -= 20;
+  if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
+  else ball->degrees2 = ball->angle;
+
+  if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang;              //45 con 8 ruote
+  else ball->dir = ball->angle - ball->plusang;                                 //45 con 8 ruote
+
+  if(ball->dir < 0) ball->dir = ball->dir + 360;
+  else ball->dir = ball->dir;
+  ball->b = ball->dir;
+  drive->prepareDrive(ball->dir, 300, 0);
+ /*  drive->speed = 300;
+  drive->dir = drive->dir; */
+  /* if(ball->angle >= 350 || ball->angle <= 10)  {
     if(ball->distance > 190) atk_direction = 0;
     else atk_direction = ball->angle;
     atk_speed = GOALIE_ATKSPD_FRT;
@@ -62,7 +75,7 @@ void Goalie::goalie() {
     atk_speed = GOALIE_ATKSPD_STRK;                                             //dove i gigahertz hanno fallito
     drive->prepareDrive(atk_direction, atk_speed, cstorc);
   }
-  else  drive->prepareDrive(atk_direction, atk_speed);
+  else  drive->prepareDrive(atk_direction, atk_speed); */
 }
 
 void Goalie::storcimentoPorta() {
@@ -73,18 +86,15 @@ void Goalie::storcimentoPorta() {
 }
 
 void Goalie::ballBack() {
-  int degrees2;
-  int dir;
 
-  int plusang;
-  if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK;
-  else plusang = 0;
+  if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK;
+  else ball->plusang = 0;
   
-  if(ball->angle > 180) degrees2 = ball->angle - 360;
-  else degrees2 = ball->angle;
-  if(degrees2 > 0) dir = ball->angle + plusang;              //45 con 8 ruote
-  else dir = ball->angle - plusang;                                 //45 con 8 ruote
-  if(dir < 0) dir = dir + 360;
-  else dir = dir;
-  atk_direction = dir;
+  if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
+  else ball->degrees2 = ball->angle;
+  if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang;              //45 con 8 ruote
+  else ball->dir = ball->angle - ball->plusang;                                 //45 con 8 ruote
+  if(ball->dir < 0) ball->dir = ball->dir + 360;
+  else ball->dir = ball->dir;
+  atk_direction = ball->dir;
 }
\ No newline at end of file