From 410ff277085a213f46d13eb997933d1b39035c99 Mon Sep 17 00:00:00 2001
From: emamaker <emamaker0@gmail.com>
Date: Thu, 9 Jun 2022 10:03:19 +0200
Subject: [PATCH] striker: orbital striker with lines

---
 src/strategy_roles/games.cpp   | 15 ++++++++++---
 src/strategy_roles/striker.cpp | 40 ++++++++++++----------------------
 2 files changed, 26 insertions(+), 29 deletions(-)

diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp
index 31f6b4b..0ce8b33 100644
--- a/src/strategy_roles/games.cpp
+++ b/src/strategy_roles/games.cpp
@@ -8,9 +8,18 @@
 #include "strategy_roles/games.h"
 
 void initGames(){
-    vector<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
-    vector<DataSource*> lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
+    std::vector<DataSource*> lIn;
+    lIn.push_back(new DataSource(S1I, true));
+    lIn.push_back(new DataSource(S2I, true));
+    lIn.push_back(new DataSource(S3I, true));
+    lIn.push_back(new DataSource(S4I, true));
+
+    std::vector<DataSource*> lOut;
+    lOut.push_back(new DataSource(S1O, true));
+    lOut.push_back(new DataSource(S2O, true));
+    lOut.push_back(new DataSource(S3O, true));
+    lOut.push_back(new DataSource(S4O, true));
 
     striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
-    keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
+    keeper = new Keeper(new LineSystemEmpty(), new PositionSystemEmpty());
 }
\ No newline at end of file
diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp
index a7c8f90..68043ef 100644
--- a/src/strategy_roles/striker.cpp
+++ b/src/strategy_roles/striker.cpp
@@ -22,47 +22,34 @@ void Striker::init()
   atk_speed = 0;
   atk_direction = 0;
   atk_tilt = 0;
+  ball_angle_filter = 0;
 
   gotta_tilt = false;
+  ball_filter = new ComplementaryFilter(1);
 }
 
 void Striker::realPlay()
 {
+  ball_angle_filter = ball_filter->calculate(CURRENT_DATA_READ.ballAngle);
+
   if (CURRENT_DATA_READ.ballSeen)
     this->striker();
   else{
     ps->goCenter();
-    roller->speed(roller->MIN);
   }
 }
 
 float ctilt = 0;
 unsigned long ttilt = 0;
 
-
-/*void Striker::striker(){
-  //seguo palla
-  if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
-  else roller->speed(roller->MIN);
-
-  int ball_angle = CURRENT_DATA_READ.ballAngleFix;
-  if(ball_angle > 180) ball_angle -= 360;
-
-  if(!ball->isInMouth())ttilt=millis();
-
-  // int tmp_ball_tilt = (0.25f*ball_angle+old_ball_Angle*0.75f);
-  // ball_tilt = ball->isInMouth() ? ball_angle : ballTilt();
-
-  // // drive->prepareDrive(0,30,ball->isInMouth() && roller->roller_armed ? tilt() : ballTilt());
-  drive->prepareDrive(0,30, millis() - ttilt > 250 ? tilt():  ball_angle);
-
-  // old_ball_Angle = ball_angle;
-  // old_ball_tilt = (int) ball_tilt;
-}*/
-
 void Striker::striker(){
+  if(CURRENT_DATA_READ.ballDistance >= 125){
+    drive->prepareDrive(ball_angle_filter > 180 ? ball_angle_filter*0.96 : ball_angle_filter*1.04, MAX_VEL_3QUARTERS, 0);
+
+  }else
+{
   //seguo palla
-  int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
+  int ball_degrees2, dir, ball_deg = ball_angle_filter, plusang = STRIKER_PLUSANG;
   
   if(ball_deg >= 344 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE;            //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
   if(ball_deg > 180) ball_degrees2 = ball_deg - 360;            //ragiono in +180 -180  
@@ -73,10 +60,11 @@ void Striker::striker(){
 
   dir = (dir + 360) % 360;
   // drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
-  drive->prepareDrive(dir, MAX_VEL_HALF, 0);
+  drive->prepareDrive(dir, MAX_VEL_3QUARTERS, CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0);
 
-  if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
-  else roller->speed(roller->MIN);
+  // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
+  // else roller->speed(roller->MIN);
+  }
   
 }