pos-sys-cam: better tresholds for considering data invalid

Also this position system already works with the PID positioning already in place

include/sensors/data_source_ball.h

@@ -2,8 +2,8 @@
 #include "behaviour_control/data_source.h"
 #include <Arduino.h>
 
-#define MOUTH_MIN_ANGLE 350
-#define MOUTH_MAX_ANGLE 10
+#define MOUTH_MIN_ANGLE 345
+#define MOUTH_MAX_ANGLE 15
 #define MOUTH_DISTANCE 100
 #define MOUTH_MAX_DISTANCE 140
 

include/sensors/data_source_camera_conicmirror.h

@@ -1,9 +1,8 @@
 #pragma once
 
-#include <Arduino.h>
-#include "behaviour_control/data_source.h"
 #include "behaviour_control/complementary_filter.h"
-
+#include "behaviour_control/data_source.h"
+#include <Arduino.h>
 
 #define FILTER_DEFAULT_COEFF 0.85
 #define FILTER_YANGLE_COEFF FILTER_DEFAULT_COEFF
@@ -13,24 +12,24 @@
 #define FILTER_YDIST_COEFF FILTER_DEFAULT_COEFF
 #define FILTER_BDIST_COEFF FILTER_DEFAULT_COEFF
 
-class DataSourceCameraConic : public DataSource{
+class DataSourceCameraConic : public DataSource
+{
 
-    public:
-        DataSourceCameraConic(HardwareSerial* ser, int baud);
-        void test() override;
-        void readSensor() override;
-        void computeCoordsAngles();
-        // int getValueAtk(bool);
-        // int getValueDef(bool);
+public:
+    DataSourceCameraConic(HardwareSerial *ser, int baud);
+    void test() override;
+    void readSensor() override;
+    void computeCoordsAngles();
+    // int getValueAtk(bool);
+    // int getValueDef(bool);
 
-        int count = 0, unkn_counter = 0;
-        bool data_received = false, start = false, end = false, dash = false;
-        char current_char = 'a', start_char = 'a', end_char = 'a'; //initialize to unused values
+    int count = 0, unkn_counter = 0;
+    bool data_received = false, start = false, end = false, dash = false;
+    char current_char = 'a', start_char = 'a', end_char = 'a'; // initialize to unused values
 
-        int goalOrientation, old_goalOrientation, pAtk, pDef;
-        String s1 = "",  s2 = "";
-
-        int yangle = 0, bangle = 0, yangle_fix = 0, bangle_fix = 0, ydist = 0, bdist = 0;
-        ComplementaryFilter *filt_yangle, *filt_bangle, *filt_yangle_fix, *filt_bangle_fix, *filt_ydist, *filt_bdist;
+    int goalOrientation, old_goalOrientation, pAtk, pDef;
+    String s1 = "", s2 = "";
 
+    int yangle = 0, bangle = 0, yangle_fix = 0, bangle_fix = 0, ydist = 0, bdist = 0;
+    ComplementaryFilter *filt_yangle, *filt_bangle, *filt_yangle_fix, *filt_bangle_fix, *filt_ydist, *filt_bdist;
 };

include/strategy_roles/striker.h

@@ -7,7 +7,7 @@
 
 #define STRIKER_ATTACK_DISTANCE 110
 #define STRIKER_TILT_STOP_DISTANCE 140
-#define STRIKER_PLUSANG 35
+#define STRIKER_PLUSANG 30 // 40
 #define STRIKER_PLUSANG_VISIONCONE 7
 #define STRIKER_VEL MAX_VEL
 
@@ -28,5 +28,5 @@ private:
     int atk_speed, atk_direction = 0, atk_tilt, stato = 0, plusang_flag = 0;
     bool flag = false;
 
-    ComplementaryFilter *ball_filter;
+    ComplementaryFilter *ball_filter, *tilt_filter;
 };

src/strategy_roles/striker.cpp

@@ -19,6 +19,7 @@ Striker::Striker(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
 void Striker::init()
 {
     ball_filter = new ComplementaryFilter(0.75f);
+    tilt_filter = new ComplementaryFilter(0.9f);
 }
 
 void Striker::realPlay()
@@ -39,52 +40,56 @@ void Striker::striker()
 
     if (CURRENT_DATA_READ.lineActive != 0) flag = false;
 
-    int dir = 0, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG + 8 * (CURRENT_DATA_READ.ballDistance <= 90);
+    int dir = 0, ball_deg = ball_filter->calculate(CURRENT_DATA_READ.ballAngle);
 
-    if (CURRENT_DATA_READ.ballDistance >= 125)
+    if (CURRENT_DATA_READ.ballDistance >= 120)
     {
         drive->prepareDrive(ball_deg > 180 ? ball_deg * 0.96 : ball_deg * 1.04, STRIKER_VEL, 0);
     }
     else
     {
-        // seguo palla
-
-        if (ball->isInFront())
+        if (ball_deg >= 345 || ball_deg <= 15 || flag)
         {
             dir = 0;
-            flag = false;
+            flag = true;
+            if (ball_deg <= 330 && ball_deg >= 30) flag = false;
         }
         else
         {
-            if (!flag)
+            flag = false;
+            double ball_x = sin(radians(ball_deg)) * CURRENT_DATA_READ.ballDistance;
+            double ball_y = cos(radians(ball_deg)) * CURRENT_DATA_READ.ballDistance;
+            double ball_y_new, ball_x_new;
+            if (ball_deg <= 100 || ball_deg >= 260)
             {
-
-                if (ball_deg <= 90)
-                    dir = ball_deg + plusang;
-                else if (ball_deg >= 270)
-                    dir = ball_deg - plusang;
-                else
-                {
-                    flag = true;
-                    if (ball_deg <= 180)
-                        plusang_flag = plusang;
-                    else
-                        plusang_flag = -plusang;
-                }
+                ball_y_new = ball_y - 70;
+                ball_x_new = ball_x;
             }
             else
-                dir = ball_deg + plusang_flag;
+            {
+                flag = false;
+                ball_y_new = ball_y - 70;
+                if (ball_deg >= 180)
+                    ball_x_new = ball_x + 100;
+                else
+                    ball_x_new = ball_x - 100;
+            }
+
+            int new_angle = 90 - (int)degrees(atan2(ball_y_new, ball_x_new));
+            if (new_angle < 0) new_angle += 360;
+
+            dir = new_angle;
+            // DEBUG.println(String(ball_deg) + ", " + String(new_angle));
         }
 
         dir = (dir + 360) % 360;
-        drive->prepareDrive(dir, STRIKER_VEL, tilt());
-
-        // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
-        // else roller->speed(roller->MIN);
+        drive->prepareDrive(dir, STRIKER_VEL, tilt_filter->calculate(CURRENT_DATA_READ.atkGAngle_fix));
     }
 }
 
 int Striker::tilt()
 {
-    return CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.atkGAngle_fix : 0;
+    // return CURRENT_DATA_READ.ballAngle <= 110 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0;
+    // return CURRENT_DATA_READ.angleAtkFix;
+    return 0;
 }

src/system/positions/positionsys_camera.cpp

@@ -60,7 +60,7 @@ void PositionSysCamera::update()
         int posya = CAMERA_GOAL_ATK_Y - distya;
         int posyd = CAMERA_GOAL_DEF_Y + distyd;
 
-        if ((distxd * distxa < 0) || (CURRENT_DATA_READ.atkSeen && CURRENT_DATA_READ.defSeen && posya - posyd > 25)) data_valid = false;
+        if (((distxd * distxa < 0) && abs(distxd - distxa) > 10) || (CURRENT_DATA_READ.atkSeen && CURRENT_DATA_READ.defSeen && abs(posya - posyd) > 25)) data_valid = false;
 
         if (data_valid)
         {