From 3308c26ef4438e8a4d51c95be3b349e1865a095c Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Wed, 14 Apr 2021 14:58:15 +0200
Subject: [PATCH] striker: robot tilt towards the goal when attacking Although
 without a roller the robot just sky-rockets the ball away when they come in
 touch

---
 include/strategy_roles/striker.h | 11 ++++++-----
 src/strategy_roles/striker.cpp   | 18 ++++++++++++++----
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h
index 0f3b29b..f2e8aa7 100644
--- a/include/strategy_roles/striker.h
+++ b/include/strategy_roles/striker.h
@@ -4,9 +4,9 @@
 #include "sensors/sensors.h"
 #include "strategy_roles/game.h"
 
-#define STRIKER_ATTACK_DISTANCE 300
+#define STRIKER_ATTACK_DISTANCE 100
 #define STRIKER_PLUSANG 55
-#define STRIKER_PLUSANG_VISIONCONE 20
+#define STRIKER_PLUSANG_VISIONCONE 10
 
 class Striker : public Game{
 
@@ -18,11 +18,12 @@ class Striker : public Game{
         void realPlay() override;
         void init() override;
         void striker();
-        void ballBack(); 
-        void storcimentoPorta();
+        int tilt();
 
-        int atk_speed, atk_direction;
+        int atk_speed, atk_direction, atk_tilt;
         float cstorc;
 
+        ComplementaryFilter* filter;
+
     
 };
diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp
index 3f0d6f3..0ac7414 100644
--- a/src/strategy_roles/striker.cpp
+++ b/src/strategy_roles/striker.cpp
@@ -20,7 +20,10 @@ void Striker::init()
 {
   atk_speed = 0;
   atk_direction = 0;
+  atk_tilt = 0;
   cstorc = 0;
+
+  filter = new ComplementaryFilter(0.7);
 }
 
 void Striker::realPlay()
@@ -37,10 +40,9 @@ void Striker::striker()
 
   if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE)
   {
-    drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 10 : CURRENT_DATA_READ.ballAngle + 10, 100, 0);
+    drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_EIGTH, 0);
     return;
   }
-  
 
   if (ball_deg > 340 || ball_deg < 20)
     plusang -= STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
@@ -58,9 +60,17 @@ void Striker::striker()
     dir = dir + 360; //se sto nel quadrante negativo ricappotto
   else
     dir = dir;
-  drive->prepareDrive(dir, MAX_VEL_EIGTH, 0);
+
+  drive->prepareDrive(dir, MAX_VEL_QUARTER, tilt());
 }
 
-void Striker::storcimentoPorta()
+int Striker::tilt()
 {
+  if (!CURRENT_DATA_READ.atkSeen) return 0;
+  if (CURRENT_DATA_READ.ballAngleFix >= 350 || CURRENT_DATA_READ.ballAngleFix <= 10)
+    atk_tilt =  (constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45) + 360) % 360;
+  else if((CURRENT_DATA_READ.ballAngleFix > 345 && CURRENT_DATA_READ.ballAngleFix < 350) || (CURRENT_DATA_READ.ballAngleFix > 10 && CURRENT_DATA_READ.ballAngleFix < 15))
+    atk_tilt = 0;
+  atk_tilt = filter->calculate(atk_tilt);
+  return atk_tilt;
 }
\ No newline at end of file