camera: coords filtering and better route calculation

include/sensors/data_source_camera_conicmirror.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "behaviour_control/data_source.h"
+#include "behaviour_control/complementary_filter.h"
 
 #define startp 105
 #define endp 115
@@ -11,6 +12,12 @@
 //Imu To Camera Angle Mult
 #define IMUTOC_AMULT 1
 
+#define FILTER_DEFAULT_COEFF 0.6
+#define FILTER_BY_COEFF FILTER_DEFAULT_COEFF
+#define FILTER_BX_COEFF FILTER_DEFAULT_COEFF
+#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF
+#define FILTER_YX_COEFF FILTER_DEFAULT_COEFF
+
 class DataSourceCameraConic : public DataSource{
 
     public:
@@ -30,4 +37,6 @@ class DataSourceCameraConic : public DataSource{
 
         int goalOrientation, pAtk, pDef;
         int value;
+
+        ComplementaryFilter *filter_yy, *filter_xy, *filter_yb, *filter_xb, *filter_yy_fix, *filter_xy_fix, *filter_yb_fix, *filter_xb_fix;
 };

include/systems/position/positionsys_camera.h

@@ -7,8 +7,8 @@
 /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
 To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
 These values need to be subtracted from the coords used in setMoveSetpoints*/
-#define CAMERA_TRANSLATION_X 8
+#define CAMERA_TRANSLATION_X 10
-#define CAMERA_TRANSLATION_Y 10
+#define CAMERA_TRANSLATION_Y 0
 //Camera center: those setpoints correspond to what we consider the center of the field
 #define CAMERA_CENTER_X 0
 #define CAMERA_CENTER_Y 0
@@ -24,7 +24,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 //Actually it's ± MAX_VAL
 #define MAX_X 50
 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
-#define DIST_MULT 1.4
+#define DIST_MULT 1.7
 
 #define Kpx 1
 #define Kix 0

src/sensors/data_source_camera_conicmirror.cpp

@@ -1,6 +1,9 @@
 #include "behaviour_control/status_vector.h"
 #include "sensors/data_source_camera_conicmirror.h"
 
+//Comment out to disable complementary filters on angles
+#define CAMERA_CONIC_FILTER_POINTS
+
 DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) {
   true_xb = 0;
   true_yb = 0;
@@ -27,6 +30,15 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D
   true_yb_fixed = 0;
   true_xy_fixed = 0;
   true_yy_fixed = 0;
+
+  filter_yy = new ComplementaryFilter(FILTER_YY_COEFF);
+  filter_xy = new ComplementaryFilter(FILTER_YX_COEFF);
+  filter_yb = new ComplementaryFilter(FILTER_BY_COEFF);
+  filter_xb = new ComplementaryFilter(FILTER_BX_COEFF);
+  filter_yy_fix = new ComplementaryFilter(FILTER_YY_COEFF);
+  filter_xy_fix = new ComplementaryFilter(FILTER_YX_COEFF);
+  filter_yb_fix = new ComplementaryFilter(FILTER_BY_COEFF);
+  filter_xb_fix = new ComplementaryFilter(FILTER_BX_COEFF);
 }
 
 void DataSourceCameraConic ::readSensor() {
@@ -76,6 +88,13 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   true_xy = 50 - true_xy;
   true_yy = true_yy - 50;
 
+  #ifdef CAMERA_CONIC_FILTER_POINTS
+  true_xb = filter_xb->calculate(true_xb);
+  true_yb = filter_yb->calculate(true_yb);
+  true_xy = filter_xy->calculate(true_xy);
+  true_yy = filter_yy->calculate(true_yy);
+  #endif
+
   //-90 + to account for phase shifting with goniometric circle
   yAngle = -90 + (atan2(true_yy, true_xy) * 180 / 3.14);
   bAngle = -90 + (atan2(true_yb, true_xb) * 180 / 3.14);
@@ -104,6 +123,13 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
   bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
 
+  #ifdef CAMERA_CONIC_FILTER_POINTS
+  true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed);
+  true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed);
+  true_xy_fixed = filter_xy_fix->calculate(true_xy_fixed);
+  true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed);
+  #endif
+
   //Important: update status vector
   CURRENT_INPUT_WRITE.cameraByte = value;
   CURRENT_DATA_WRITE.cam_xb = true_xb;

src/system/positions/positionsys_camera.cpp

@@ -2,6 +2,7 @@
 #include "systems/position/positionsys_camera.h"
 #include "sensors/sensors.h"
 #include "vars.h"
+#include "math.h"
 
 PositionSysCamera::PositionSysCamera() {
     MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
@@ -119,10 +120,11 @@ void PositionSysCamera::CameraPID(){
         int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
         dir = (dir+360) % 360;
 
-        int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
+        int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
+        // int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
         int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0,  MAX_VEL);
         speed = filterSpeed->calculate(speed);
-        speed = speed > 35 ? speed : 0;
+        speed = speed > 40 ? speed : 0;
         dir = filterDir->calculate(dir);
         // drive->prepareDrive(dir, speed, 0);
 

src/test_menu.cpp

@@ -13,6 +13,8 @@
 #include "behaviour_control/data_source.h"
 #include "behaviour_control/status_vector.h"
 
+unsigned long timer = 0;
+
 void TestMenu::testMenu()
 {
     if (!(DEBUG.available() || flagtest))
@@ -91,6 +93,16 @@ void TestMenu::testMenu()
             DEBUG.println(robot_indentifier);
             delay(50);
             break;
+        case 'p':
+            if(millis() - timer > 150){
+                timer = millis();
+                DEBUG.println("Position on the field (camera)");
+                DEBUG.print("X: ");
+                DEBUG.print(CURRENT_DATA_READ.posx);
+                DEBUG.print(" , Y: ");
+                DEBUG.println(CURRENT_DATA_READ.posy);
+            }
+            break;
         case 'u':
             DEBUG.println("32u4 receive Test");
             while (Serial2.available())

utility/OpenMV/conic_eff_h7.py

@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (48, 73, -15, 15, 29, 79),    # thresholds yellow goal
+thresholds = [  (54, 73, -4, 17, 18, 73),    # thresholds yellow goal
-                (12, 43, -15, 15, -41, -9)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (36, 45, -7, 7, -35, -10)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (50,5,250, 230)
+roi = (80, 0, 240, 230)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -69,7 +69,7 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QVGA)
 sensor.set_windowing(roi)
 sensor.set_contrast(0)
-sensor.set_saturation(3)
+sensor.set_saturation(0)
 sensor.set_brightness(1)
 sensor.set_auto_whitebal(True)
 sensor.set_auto_exposure(False, 6576)