From c168d0a1b127f97bc3a75fa85ba03bdeecd44d6e Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Tue, 22 Jun 2021 14:55:12 +0200
Subject: [PATCH] precision shooter: spinning kick finally precise

---
 include/motors_movement/drivecontroller.h     |  2 +-
 .../sensors/data_source_camera_conicmirror.h  |  4 +--
 include/strategy_roles/precision_shooter.h    |  3 +-
 include/systems/position/positionsys_camera.h |  2 +-
 src/strategy_roles/precision_shooter.cpp      | 30 ++++++++++++-------
 5 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h
index 9c1f253..edbca77 100644
--- a/include/motors_movement/drivecontroller.h
+++ b/include/motors_movement/drivecontroller.h
@@ -20,7 +20,7 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 60
+#define MAX_VEL 70
 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
 #define MAX_VEL_HALF ((int)MAX_VEL*0.5)
 #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)
diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h
index 33621be..51a539b 100644
--- a/include/sensors/data_source_camera_conicmirror.h
+++ b/include/sensors/data_source_camera_conicmirror.h
@@ -27,8 +27,8 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 // #define CAMERA_TRANSLATION_Y 7
 
 //Robot with roller
-#define CAMERA_TRANSLATION_X 0
-#define CAMERA_TRANSLATION_Y 0
+#define CAMERA_TRANSLATION_X -8
+#define CAMERA_TRANSLATION_Y 4
 
 class DataSourceCameraConic : public DataSource{
 
diff --git a/include/strategy_roles/precision_shooter.h b/include/strategy_roles/precision_shooter.h
index 74821f2..0fa38b4 100644
--- a/include/strategy_roles/precision_shooter.h
+++ b/include/strategy_roles/precision_shooter.h
@@ -11,8 +11,9 @@
 #define PS_PLUSANG_VISIONCONE 10
 
 // There needs to be a little bit of space between the target point and the spot to be in
-#define PS_SPINNER_OVERHEAD 10
+#define PS_SPINNER_OVERHEAD 7
 
+#define KICK_LIMIT_TILT1 200
 #define KICK_LIMIT_MAX 315
 #define KICK_LIMIT_MIN 45
 
diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h
index f842e99..30eb46e 100644
--- a/include/systems/position/positionsys_camera.h
+++ b/include/systems/position/positionsys_camera.h
@@ -20,7 +20,7 @@
 //Actually it's ± MAX_VAL
 #define MAX_X 50
 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
-#define DIST_MULT 3.5
+#define DIST_MULT 5
 
 #define VICINITY_DIST_TRESH 2
 
diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp
index bf495a3..b69caed 100644
--- a/src/strategy_roles/precision_shooter.cpp
+++ b/src/strategy_roles/precision_shooter.cpp
@@ -39,11 +39,13 @@ int ball_catch_state = 0;
 bool ball_catch_flag = false;
 unsigned long ball_catch_timer = 0;
 float ball_catch_tilt = 0;
+int limitx = 0;
 
 void PrecisionShooter::realPlay()
 {
   if (CURRENT_DATA_READ.ballSeen)
-    this->catchBall();
+    this->spinner(CURRENT_DATA_READ.xAtkFix);
+    // this->catchBall();
   else{
     ps->goCenter();
     ball_catch_flag = false;
@@ -80,8 +82,8 @@ void PrecisionShooter::spinner(int targetx){
   }else if(spinner_state == 1){
     roller->speed(roller->MAX);
 
-    int spotx;
-    if(targetx > 0) spotx = targetx-PS_SPINNER_OVERHEAD;
+    int spotx = targetx;
+    if(targetx >= 0) spotx = targetx-PS_SPINNER_OVERHEAD;
     else spotx = targetx+PS_SPINNER_OVERHEAD;
 
     if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) {
@@ -97,12 +99,16 @@ void PrecisionShooter::spinner(int targetx){
         spinner_tilt = CURRENT_DATA_READ.IMUAngle;
       }
 
-      if(CURRENT_DATA_READ.posx > targetx){
+      if(targetx >= 0) {
         tilt1 = -0.15;
-        tilt2 = 0.3;
+        tilt2 = 0.55;
+
+        limitx = 360-KICK_LIMIT_TILT1;
       }else{
         tilt1 = 0.15;
-        tilt2 = -0.3;
+        tilt2 = -0.55;
+
+        limitx = KICK_LIMIT_TILT1;
       }
     
     }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
@@ -112,21 +118,23 @@ void PrecisionShooter::spinner(int targetx){
     spinner_tilt += tilt1;
     drive->prepareDrive(0,0,spinner_tilt);
     
-    if(CURRENT_DATA_READ.IMUAngle > 175 && CURRENT_DATA_READ.IMUAngle < 185) {
+    if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
       spinner_timer = millis();
       spinner_state=3;
     }
   }else if(spinner_state == 3){
     roller->speed(roller->MAX);
     drive->prepareDrive(0,0,spinner_tilt);
-    if(millis() - spinner_timer > 1000) {
+    if(millis() - spinner_timer > 150) {
       spinner_state=4;
       spinner_tilt = CURRENT_DATA_READ.IMUAngle;
     }
   }else if(spinner_state == 4){
-    spinner_tilt += tilt2;
-    spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
-    drive->prepareDrive(0,0,spinner_tilt);
+    drive->prepareDrive(0,0,0);
+
+    // spinner_tilt += tilt2;
+    // spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
+    // drive->prepareDrive(0,0,spinner_tilt);
 
     if(CURRENT_DATA_READ.IMUAngle >= KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= KICK_LIMIT_MIN) roller->speed(roller->MIN);
   }