From 4e3bf45bcb519d33a36f8edf7caa47f1c7b29abd Mon Sep 17 00:00:00 2001
From: u-siri-ous <sanninosiria@gmail.com>
Date: Thu, 5 Dec 2019 12:53:14 +0100
Subject: [PATCH] sbubabub

---
 include/goalie.h | 26 ++++++++++++--
 src/goalie.cpp   | 94 ++++++++++++++++++++++++++++++++++++++++++++----
 src/main.cpp     | 83 ++++++++++++++++++++++++++++++++++++++++--
 src/motor.cpp    |  2 --
 4 files changed, 191 insertions(+), 14 deletions(-)

diff --git a/include/goalie.h b/include/goalie.h
index 029fe32..a5044a5 100644
--- a/include/goalie.h
+++ b/include/goalie.h
@@ -2,12 +2,34 @@
 
 #include "game.h"
 
+#define GOALIE_ATKSPD_LAT  255
+#define GOALIE_ATKSPD_BAK  350
+#define GOALIE_ATKSPD_FRT  345
+#define GOALIE_ATKSPD_STRK 355
+#define GOALIE_ATKDIR_PLUSANG1 20
+#define GOALIE_ATKDIR_PLUSANG2 35
+#define GOALIE_ATKDIR_PLUSANG3 40
+#define GOALIE_ATKDIR_PLUSANGBAK 40
+#define GOALIE_ATKDIR_PLUSANG1_COR 60
+#define GOALIE_ATKDIR_PLUSANG2_COR 70
+#define GOALIE_ATKDIR_PLUSANG3_COR 70
+
 class Goalie{
 
     public:
-        Goalie();
+    void goalie();
+    void palla_dietro();
+    void palla_dietroP();
+    void storcimentoPorta();
+    void storcimentoPorta2();
+    void storcimentoZone();
+    void storcimentoPortaIncr();
+    void leaveMeAlone();
+    void ballBack();
+    int atk_direction, atk_speed;
+      /*   Goalie();
         void goalie();
-        void ballBack();
+        void ballBack(); */
         //void rigore(); to be implemented
 
 };
diff --git a/src/goalie.cpp b/src/goalie.cpp
index d99cf56..3d0cfe6 100644
--- a/src/goalie.cpp
+++ b/src/goalie.cpp
@@ -1,11 +1,79 @@
 #include "goalie.h"
 
-Goalie::Goalie(){
+/* Goalie::Goalie(){
+    digitalWrite(LED_Y, LOW);
+} */
 
-}
+/* void Goalie::goalie(){
+    digitalWrite(LED_Y, LOW);
+  if(ball->angle >= 350 || ball->angle <= 10)  {
+    if(ball->distance > 190) atk_direction = 0;
+    else atk_direction = ball->angle;
+    atk_speed = GOALIE_ATKSPD_FRT;
+  }
 
-void Goalie::goalie(){
-    digitalWrite(LED_G, HIGH);
+  if(ball->angle >= 90 && ball->angle <= 270) {
+    ballBack();
+    atk_speed = GOALIE_ATKSPD_BAK;                   
+  }
+  if(digitalRead(SWITCH_DX) == 1) {
+    if(ball->angle > 10   && ball->angle <  30)   {
+      atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 30 && ball->angle <  45)    {
+      atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 45 && ball->angle <  90)    {
+      atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 270 && ball->angle <= 315)   {
+      atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 315 && ball->angle <= 330)   {
+      atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 330 && ball->angle <  350)   {
+      atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+  } else {
+    if(ball->angle > 10   && ball->angle <  30)   {
+      atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 30 && ball->angle <  45)    {
+      atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 45 && ball->angle <  90)    {
+      atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 270 && ball->angle <= 315)   {
+      atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 315 && ball->angle <= 330)   {
+      atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 330 && ball->angle <  350)   {
+      atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+  }
+
+  if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) {      //storcimento
+    atk_speed = GOALIE_ATKSPD_STRK;                                             //dove i gigahertz hanno fallito
+    drive->prepareDrive(atk_direction, atk_speed, 0);
+  }
+  else drive->prepareDrive(atk_direction, atk_speed, 0);
+   /*  digitalWrite(LED_G, HIGH);
     if(ball->ballSeen){
         if(ball->angle >= 0 && ball->angle < 45) drive->drive(ball->angle, 75, 0);
         else if(ball->angle >= 45 && ball->angle <= 60) drive->drive(ball->angle, 75, 0);
@@ -14,9 +82,21 @@ void Goalie::goalie(){
     } else {
         drive->prepareDrive(0, 0, 0);
         drive->drive(0, 0, 0);
-    }
-}
+    } */
 
 void Goalie::ballBack(){
-
+    int ballangle2;
+    int dir;
+    int plusang;
+    
+  if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK;
+  else plusang = 0;
+  
+  if(ball->angle > 180) ballangle2 = ball->angle - 360;
+  else ballangle2 = ball->angle;
+  if(ballangle2 > 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;
 }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 77614af..9324dd1 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,6 +1,82 @@
 #include <Arduino.h>
 
 #include "game.h"
+#include "goalie.h"
+
+class Trial{
+  public:
+  void trial(){
+    digitalWrite(LED_Y, LOW);
+  if(ball->angle >= 350 || ball->angle <= 10)  {
+    if(ball->distance > 190) goalie->atk_direction = 0;
+    else goalie->atk_direction = ball->angle;
+    goalie->atk_speed = GOALIE_ATKSPD_FRT;
+  }
+
+  if(ball->angle >= 90 && ball->angle <= 270) {
+    goalie->ballBack();
+    goalie->atk_speed = GOALIE_ATKSPD_BAK;                   
+  }
+  if(digitalRead(SWITCH_DX) == 1) {
+    if(ball->angle > 10   && ball->angle <  30)   {
+      goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 30 && ball->angle <  45)    {
+      goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 45 && ball->angle <  90)    {
+      goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 270 && ball->angle <= 315)   {
+      goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 315 && ball->angle <= 330)   {
+      goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 330 && ball->angle <  350)   {
+      goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+  } else {
+    if(ball->angle > 10   && ball->angle <  30)   {
+      goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1_COR;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 30 && ball->angle <  45)    {
+      goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2_COR;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle >= 45 && ball->angle <  90)    {
+      goalie->atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3_COR;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 270 && ball->angle <= 315)   {
+      goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 315 && ball->angle <= 330)   {
+      goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(ball->angle > 330 && ball->angle <  350)   {
+      goalie->atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1;
+      goalie->atk_speed = GOALIE_ATKSPD_LAT;
+    }
+  }
+
+  if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) {      //storcimento
+    goalie->atk_speed = GOALIE_ATKSPD_STRK;                                             //dove i gigahertz hanno fallito
+    drive->prepareDrive(goalie->atk_direction, goalie->atk_speed, 0);
+  }
+  else drive->prepareDrive(goalie->atk_direction, goalie->atk_speed, 0);
+}
+
+};
 
 
 void setup() {
@@ -13,9 +89,10 @@ void setup() {
 
 void loop() {
   updateSensors();
-
-  drive->prepareDrive(0,0,0);
-  linesCtrl->update();
+  Trial trial;
+  trial.trial();
+  //drive->prepareDrive(0,0,0);
+  //linesCtrl->update();
  
   drive->drivePrepared();
 
diff --git a/src/motor.cpp b/src/motor.cpp
index 0bf5f13..ccdf0ec 100644
--- a/src/motor.cpp
+++ b/src/motor.cpp
@@ -11,8 +11,6 @@ Motor::Motor(int a, int b, int pwm, int angle_){
     pinMode(pinA, OUTPUT);
     pinMode(pinB, OUTPUT);
     pinMode(pinPwm, OUTPUT);
-
-    angle = 0;
 }
 
 Motor::Motor(){ }