From 75610d3dde07ae8a358d2f12cde1c8035482cea6 Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Wed, 4 Dec 2019 17:25:25 +0100
Subject: [PATCH] first trial to follow the ball

---
 include/game.h |  1 +
 src/game.cpp   | 11 +++++++++--
 src/main.cpp   |  8 ++++----
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/include/game.h b/include/game.h
index 8eaf387..4421355 100644
--- a/include/game.h
+++ b/include/game.h
@@ -13,6 +13,7 @@ class Game{
         Game();
         void keeper();
         void goalie();
+        void ballBack();
         bool role, attackGoal; //1->goalie 0->keeper, 1->yellow 0->blue
         //~Game();
 };
\ No newline at end of file
diff --git a/src/game.cpp b/src/game.cpp
index aa8be28..b71998c 100644
--- a/src/game.cpp
+++ b/src/game.cpp
@@ -9,8 +9,11 @@ void Game::goalie(){
     //drive->prepareDrive(0, 150, 0);
     digitalWrite(LED_G, HIGH);
     if(ball->ballSeen){
-        if(ball->angle >= 0) drive->drive(0, 100, 0);
-        else if(ball->angle <= 45) drive->drive(45, 100, 0);
+        if(ball->angle >= 0 && ball->angle <= 45) drive->drive(0, 75, 0);
+        //else if(ball->angle <= 45) drive->drive(45, 75, 0);
+        else if(ball->angle > 45 && ball->angle <= 90) drive->drive(90, 75, 0);
+        //else if(ball->angle <= 135) drive->drive(135, 75, 0);
+        else if(ball->angle > 90 && ball->angle <= 180) drive->drive(180, 75, 0);
     } else {
         drive->prepareDrive(0, 0, 0);
         drive->drive(0, 0, 0);
@@ -24,4 +27,8 @@ void Game::keeper(){
     drive->drive(0, 0, 0);
 }
 
+void ballBack(){
+
+}
+
 
diff --git a/src/main.cpp b/src/main.cpp
index 5084296..4cc77e7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -15,12 +15,12 @@ void loop() {
   //updateSensors();
   /*if(millis() % 100 == 0)
   DEBUG_PRINT.println(us->getValue());*/
-  /* if(game->role) game->goalie();
-  else game->keeper(); */
-  game->goalie();
   game->role = digitalRead(SWITCH_DX);
   game->attackGoal = digitalRead(SWITCH_SX);
+  if(game->role) game->goalie();
+  else game->keeper();
+  //game->goalie();
+  //drive->drivePrepared();
   //compass->test();
   //usCtrl->test();
-  delay(200);
 }
\ No newline at end of file