From 05399d8557766b5bbfa4961efba0159a458695b5 Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Wed, 22 Jan 2020 18:51:30 +0100
Subject: [PATCH] new goalie c:

---
 include/data_source_ball.h |  2 +-
 include/goalie.h           |  2 +-
 src/data_source_camera.cpp |  6 +++---
 src/goalie.cpp             | 29 ++++++++++++++++-------------
 src/main.cpp               | 10 ++++++----
 5 files changed, 27 insertions(+), 22 deletions(-)

diff --git a/include/data_source_ball.h b/include/data_source_ball.h
index cfb47f5..fdffeff 100755
--- a/include/data_source_ball.h
+++ b/include/data_source_ball.h
@@ -10,5 +10,5 @@ class DataSourceBall : public DataSource{
         
         int angle, distance;
         bool ballSeen;
-        int plusang, dir, degrees2,b;
+        int dir, degrees2,b;
 }; 
\ No newline at end of file
diff --git a/include/goalie.h b/include/goalie.h
index 619a273..60d495d 100755
--- a/include/goalie.h
+++ b/include/goalie.h
@@ -25,7 +25,7 @@ class Goalie : public Game{
     private:
         void realPlay() override;
         void init() override;
-        void goalie();
+        void goalie(int);
         void ballBack(); 
         void storcimentoPorta();
 
diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index 9a56d83..d64e95b 100755
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -15,7 +15,7 @@ void DataSourceCamera :: readSensor(){
       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;
@@ -25,8 +25,8 @@ void DataSourceCamera :: readSensor(){
     }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++;
       }
diff --git a/src/goalie.cpp b/src/goalie.cpp
index 5ce4cf9..4fadbd0 100755
--- a/src/goalie.cpp
+++ b/src/goalie.cpp
@@ -17,22 +17,25 @@ void Goalie::init(){
 }
 
 void Goalie::realPlay(){
-  if(ball->ballSeen) this->goalie();
+  if(ball->ballSeen) this->goalie(50);
   else drive->prepareDrive(0,0,0);
 }
 
-void Goalie::goalie() {
-  if(ball->angle > 340 || ball->angle < 20) ball->plusang -= 20;
-  if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
-  else ball->degrees2 = ball->angle;
+void Goalie::goalie(int plusang) {
+  if(ball->distance < 185) drive->prepareDrive(ball->angle, 350, 0);
+  else{
+    if(ball->angle > 340 || ball->angle < 20) 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->degrees2 > 0) ball->dir = ball->angle + plusang;              //45 con 8 ruote
+    else ball->dir = ball->angle - 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);
+    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)  {
@@ -87,7 +90,7 @@ void Goalie::storcimentoPorta() {
 
 void Goalie::ballBack() {
 
-  if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK;
+/*   if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK;
   else ball->plusang = 0;
   
   if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
@@ -96,5 +99,5 @@ void Goalie::ballBack() {
   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;
+  atk_direction = ball->dir; */
 }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 7e62f00..d7a3975 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -16,11 +16,13 @@ void setup() {
 
 void loop() {
   updateSensors();
-  camera->test();
-  /* 
+  //camera->test();
   goalie->play(role==1);
-  keeper->play(role==0); */
+  keeper->play(role==0);  
+
+  ball->test();
+
 
   // Last thing to do: movement  
-/*   drive->drivePrepared();  */
+  drive->drivePrepared();  
 }