New conic shaped mirror, works better. It's higher and has more noticeable changes on the y axis

include/data_source_camera_conicmirror.h

@@ -4,10 +4,10 @@
 
 #define startp 105
 #define endp 115
+#define unkn 116
 //Coords are mapped from 0 up to this value
 #define MAP_MAX 100
 #define HALF_MAP_MAX  50
-//#define unkn 0b01101001 
 
 class DataSourceCameraConic : public DataSource{
 

include/drivecontroller.h

@@ -6,9 +6,17 @@
 
 //PID Constants
 #define KP 1.5
-#define KI 0
+#define KI 0.2
 #define KD 0.1
 
+#define KSPD 0.3
+
+//BEST NUMBERS YET
+//USE MOVING AVERAGE AND ANGLE WRAP
+// #define KP 1.5
+// #define KI 0
+// #define KD 0.1
+
 #define UNLOCK_THRESH 800
 
 class DriveController{
@@ -33,8 +41,7 @@ class DriveController{
         Motor* m3;
         Motor* m4;
         PID* pid;
-        int pDir, pSpeed, pTilt;
+        int pDir, pSpeed, pTilt, oldSpeed;
-        int gDir, gSpeed, gTilt;
         float x, y, vx, vy, speed1, speed2, speed3, speed4, pidfactor, delta;
 
         double input, output, setpoint;

include/goalie.h

@@ -4,6 +4,9 @@
 #include "sensors.h"
 #include "data_source_camera_vshapedmirror.h"
 
+#define TILT_MULT 1.8
+#define TILT_DIST 180
+#define CATCH_DIST 150
 #define GOALIE_ATKSPD_LAT  255
 #define GOALIE_ATKSPD_BAK  350
 #define GOALIE_ATKSPD_FRT  345

include/positionsys_camera.h

@@ -0,0 +1,14 @@
+#include "systems.h"
+
+#define CAMERA_CENTER_X 0
+#define CAMERA_CENTER_Y 0
+
+class PositionSysCamera : public PositionSystem{
+
+    public:
+        PositionSysCamera();
+        void goCenter();
+        void update() override;
+        void test() override;
+
+};

include/positionsys_zone.h

@@ -55,9 +55,6 @@
 #define SOUTH_CENTER 8
 #define SOUTH_EAST 9
 
-#define CAMERA_CENTER_X 3
-#define CAMERA_CENTER_Y 6
-
 class PositionSysZone : public PositionSystem{
     public:
         PositionSysZone();

include/sensors.h

@@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
 
 s_extr DataSourceBNO055* compass;
 s_extr DataSourceBall* ball;
-s_extr DataSourceCameraVShaped* camera;
+s_extr DataSourceCameraConic* camera;
 s_extr DriveController* drive;
 s_extr DataSourceBT* bt;
 

include/status_vector.h

@@ -35,13 +35,21 @@ typedef struct input{
 }input;
 
 typedef struct data{
-    int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix,
+    int IMUAngle, ballAngle, ballDistance, 
-        speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role, axisBlock[4];
+        yAngle, bAngle, yAngleFix, bAngleFix, 
+        yDist, bDist, 
+        angleAtk, angleAtkFix, angleDef, angleDefFix,
+        speed, tilt, dir, axisBlock[4],
+        USfr, USsx, USdx, USrr, 
+        lineOutDir, matePos, role;
     Game* game;
     LineSystem* lineSystem;
     PositionSystem* posSystem;
     byte xb, yb, xy, yy, lineSeen, lineActive;
-    bool mate, ATKgoal, DEFgoal, ballSeen;
+    bool mate, 
+        ATKgoal, DEFgoal, 
+        atkSeen, defSeen, bSeen, ySeen,
+        ballSeen;
 }data;
 
 sv_extr input inputs[dim];

include/vars.h

@@ -1,6 +1,8 @@
 #pragma once
 #define DEBUG Serial3
 
+#define GLOBAL_SPD_MULT 1.0
+
 #define LED_R 20
 #define LED_Y 17
 #define LED_G 13

src/data_source_camera_conicmirror.cpp

@@ -78,29 +78,39 @@ void DataSourceCameraConic :: readSensor(){
     CURRENT_DATA_WRITE.bAngleFix = bAngleFix;
     CURRENT_DATA_WRITE.yDist = yDist;
     CURRENT_DATA_WRITE.bDist = bDist;
+
+    if(xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false;
+    else CURRENT_DATA_WRITE.bSeen = true;
+    if(xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false;
+    else CURRENT_DATA_WRITE.ySeen = true;
+
     if(goalOrientation == HIGH){
       CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
       CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
+      CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen;
       CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
       CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
+      CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
     }else{
       CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
       CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
+      CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
       CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
       CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
+      CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
     }
   }
 }
 
 
-// int DataSourceCameraConic::getValueAtk(bool fixed){
+// int DataSource<CameraConic::getValueAtk(bool fixed){
 //   if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
 //   else return goalOrientation == HIGH ? yAngle : bAngle;
 // }
 // int DataSourceCameraConic::getValueDef(bool fixed){
 //   if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix;
 //   else return goalOrientation == LOW ? yAngle : bAngle;
-// }
+// }>
 
 void DataSourceCameraConic::test(){
   goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu

src/drivecontroller.cpp

@@ -63,6 +63,9 @@ float DriveController::torad(float f){
 
 void DriveController::drive(int dir, int speed, int tilt){
 
+    speed = (speed * KSPD + oldSpeed * (1-KSPD))*GLOBAL_SPD_MULT;
+    tilt = tilt > 180 ? tilt - 360 : tilt;
+
     vx = ((speed * cosins[dir]));
     vy = ((-speed * sins[dir]));
 
@@ -82,11 +85,11 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed4 = -(speed2);
 
     //  calcola l'errore di posizione rispetto allo 0
-    delta = compass->getValue();
+    delta = CURRENT_DATA_READ.IMUAngle;
     if(delta > 180) delta = delta - 360;
 
     input = delta;
-    setpoint = 0;
+    setpoint = tilt;
     
     pid->Compute();
 
@@ -106,6 +109,8 @@ void DriveController::drive(int dir, int speed, int tilt){
     m3->drive((int) speed3);
     m4->drive((int) speed4);
 
+    oldSpeed = speed;
+
     CURRENT_DATA_WRITE.dir = dir;
     CURRENT_DATA_WRITE.speed = speed;
     CURRENT_DATA_WRITE.tilt = tilt;

src/games.cpp

@@ -3,11 +3,12 @@
 #include "games.h"
 #include "linesys_2019.h"
 #include "positionsys_zone.h"
+#include "positionsys_camera.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) };
 
-    goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysZone());
+    goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysCamera());
-    keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysZone());
+    keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysCamera());
 }

src/goalie.cpp

@@ -3,7 +3,7 @@
 #include "vars.h"
 #include "status_vector.h"
 #include "math.h"
-#include "positionsys_zone.h"
+#include "positionsys_camera.h"
 
 Goalie::Goalie() : Game() {
   init();
@@ -20,15 +20,15 @@ void Goalie::init(){
 }
 
 void Goalie::realPlay(){
-  if(ball->ballSeen) this->goalie(45);
+  if(ball->ballSeen) this->goalie(50);
-  else ((PositionSysZone*)ps)->goCenter();
+  else ((PositionSysCamera*)ps)->goCenter();
 }
 
 int dir, degrees2;
 void Goalie::goalie(int plusang) {
-  if(ball->distance < 160) drive->prepareDrive(ball->angle, 350, 0);
+  if(ball->distance < CATCH_DIST) drive->prepareDrive(ball->angle, 350, 0);
   else{
-    if(ball->angle > 340 || ball->angle < 20) plusang *= 0.15;
+    if(ball->angle > 345 || ball->angle < 15) plusang *= 0.15;
     if(ball->angle > 180) degrees2 = ball->angle - 360;
     else degrees2 = ball->angle;
 
@@ -39,7 +39,7 @@ void Goalie::goalie(int plusang) {
     else dir = dir;
 
     storcimentoPorta();
-    if(ball->distance > 185 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
+    if(ball->distance > TILT_DIST && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
     else {
       drive->prepareDrive(dir, 350, 0);
       cstorc = 0;
@@ -48,8 +48,8 @@ void Goalie::goalie(int plusang) {
 }
 
 void Goalie::storcimentoPorta() {
-  if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9;
+  if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9;
-  else if (CURRENT_DATA_READ.angleAtkFix  <= -10 && CURRENT_DATA_READ.angleAtkFix >= -90) cstorc-=9;
+  else if (CURRENT_DATA_READ.angleAtkFix  <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9;
-  // else cstorc *= 0.7;
+  else cstorc *= 0.9;
   cstorc = constrain(cstorc, -45, 45);
 }

src/main.cpp

@@ -24,7 +24,7 @@ void loop() {
   keeper->play(role==0);  
 
   // Last thing to do: movement and update status vector
-  drive->prepareDrive(0,0,0);
+  // drive->prepareDrive(0,0,  CURRENT_DATA_READ.angleAtkFix);
   drive->drivePrepared();  
   updateStatusVector();
 } 

src/positionsys_camera.cpp

@@ -0,0 +1,43 @@
+#include "positionsys_camera.h"
+#include "status_vector.h"
+#include "vars.h"
+#include "sensors.h"
+
+PositionSysCamera::PositionSysCamera() {}
+
+void PositionSysCamera::update(){
+}
+
+void PositionSysCamera::test(){
+}
+
+void PositionSysCamera::goCenter(){
+    /*WORKS BUT CAN BE BETTER*/
+    //Y
+    if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
+    else if ((camera->true_yb + camera->true_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
+    //X
+    else if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
+    else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
+    else drive->prepareDrive(0, 0, 0);
+
+
+    /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
+    // int x = 1;
+    // int y = 1;
+    
+    // //Trying using an angle
+    // if((CURRENT_DATA_READ.yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy) < -CAMERA_CENTER_Y) y = CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy;
+    // if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xb;
+    // if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xy;
+
+    // DEBUG.print(x);
+    // DEBUG.print(":");
+    // DEBUG.println(y);
+
+    // int dir = 90-(atan2(y,x)*180/3.14);
+    // dir = (dir+360) % 360;
+    // DEBUG.println(dir);
+    // drive->prepareDrive(dir, 100, 0);
+
+}

src/positionsys_zone.cpp

@@ -374,14 +374,6 @@ void PositionSysZone::testLogicZone(){
 
 
 void PositionSysZone::goCenter() {
-  // if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
-  // else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); 
-  // else drive->prepareDrive(0, 0, 0);
-/*   if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
-  else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
-  else drive->prepareDrive(0, 0, 0); */
- /*  
- PREVIOUS
   if (zoneIndex == 8)
     drive->prepareDrive(330, GOCENTER_VEL);
   if (zoneIndex == 7)
@@ -399,7 +391,7 @@ void PositionSysZone::goCenter() {
   if (zoneIndex == 1)
     drive->prepareDrive(180, GOCENTER_VEL);
   if (zoneIndex == 0)
-    drive->prepareDrive(135, GOCENTER_VEL); */
+    drive->prepareDrive(135, GOCENTER_VEL);
 }
 
 

src/sensors.cpp

@@ -15,7 +15,7 @@ void initSensors(){
     drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     compass = new DataSourceBNO055();
     ball = new DataSourceBall(&Serial4, 57600);
-    camera = new DataSourceCameraVShaped(&Serial2, 19200);
+    camera = new DataSourceCameraConic(&Serial2, 19200);
     usCtrl = new DataSourceCtrl(dUs);
     bt = new DataSourceBT(&Serial3, 115200);
  }
@@ -26,6 +26,6 @@ void updateSensors(){
     
     compass->update();
     ball->update();
-    // camera->update();
+    camera->update();
     usCtrl->update();
 }

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (26, 74, -11, 6, 17, 50),    # thresholds yellow goal
+thresholds = [  (40, 100, -14, 21, 16, 69),    # thresholds yellow goal
-                (12, 44, -34, 42, -105, -25)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (14, 46, -11, 12, -47, -19)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -62,9 +62,9 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+3)
 sensor.set_saturation(0)
-sensor.set_brightness(-2)
+sensor.set_brightness(-1)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 10000)
+sensor.set_auto_exposure(False, 3000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
@@ -84,7 +84,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=75, area_threshold=130, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=70, merge = True):
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())
 

utility/OpenMV/dist_test.py

@@ -0,0 +1,161 @@
+# goal dist tracking with conic mirror - By: EmaMaker - fri 21 feb 2020
+# Based on:
+# color tracking - By: paolix - ven mag 18 2018
+
+# Automatic RGB565 Color Tracking Example
+#
+import sensor, image, time, pyb, math
+from pyb import UART
+uart = UART(3,19200, timeout_char = 1000)
+
+def torad(f):
+    return (f*math.pi/180) % math.pi
+
+#These measures are in centimeters
+FIELD_W = 131
+FIELD_H = 193
+GOALS_DEPTH = 207
+
+#Attack 1 means attacking yellow, attack 0 means attacking blue
+ATTACKING = 0
+
+# LED Setup ##################################################################
+
+red_led = pyb.LED(1)
+green_led = pyb.LED(2)
+blue_led = pyb.LED(3)
+
+red_led.off()
+green_led.off()
+blue_led.on()
+##############################################################################
+
+
+#thresholds = [  (30, 100, 15, 127, 15, 127),    # generic_red_thresholds
+#                (30, 100, -64, -8, -32, 32),    # generic_green_thresholds
+#                (0, 15, 0, 40, -80, -20)]       # generic_blue_thresholds
+
+#thresholds = [  (54, 93, -10, 25, 55, 70),    # thresholds yellow goal
+#                (30, 45, 1, 40, -60, -19)]    # thresholds blue goal
+#
+thresholds = [  (40, 100, -3, 35, 16, 96)  ,    # thresholds yellow goal
+                (39, 59, -13, 12, -43, -19)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+
+roi = (0, 6, 318, 152)
+
+# Camera Setup ###############################################################
+'''sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QVGA)
+sensor.skip_frames(time = 2000)
+sensor.set_auto_gain(False)         # must be turned off for color tracking
+sensor.set_auto_whitebal(False)     # must be turned off for color tracking
+sensor.set_auto_exposure(False, 10000)
+#sensor.set_backlight(1)
+#sensor.set_brightness(+2)
+#sensor.set_windowing(roi)
+clock = time.clock()'''
+
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QQVGA)
+sensor.set_contrast(+2)
+sensor.set_saturation(2)
+sensor.set_brightness(-3)
+sensor.set_quality(0)
+sensor.set_auto_exposure(False, 6000)
+sensor.set_auto_gain(True)
+sensor.skip_frames(time = 300)
+
+clock = time.clock()
+##############################################################################
+
+
+# [] list
+# () tupla
+
+'''while(True):
+    clock.tick()
+    img = sensor.snapshot()'''
+
+while(True):
+    clock.tick()
+
+    blue_led.off()
+
+    tt_yellow = [(0,999,0,1)]      ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
+    tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
+
+    img = sensor.snapshot()
+    for blob in img.find_blobs(thresholds, pixels_threshold=45, area_threshold=80, merge = True):
+        img.draw_rectangle(blob.rect())
+        img.draw_cross(blob.cx(), blob.cy())
+
+        if (blob.code() == 1):
+            tt_yellow = tt_yellow +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
+        if (blob.code() == 2):
+            tt_blue = tt_blue +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
+
+    tt_yellow.sort(key=lambda tup: tup[0])  ## ordino le liste
+    tt_blue.sort(key=lambda tup: tup[0])    ## ordino le liste
+
+    ny = len(tt_yellow)
+    nb = len(tt_blue)
+
+    '''Yellow'''
+    area,cx,cy,code = tt_yellow[ny-1]
+    cx = img.width() / 2 - cx
+    cy = img.height() / 2 - cy
+    yAngle = math.pi/2 - math.atan2(cy, cx)
+    yDist = math.sqrt(cx*cx + cy*cy)
+    string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(yAngle)+" | "+str(yDist)+str(area)+"y"
+    #print (string_yellow)   # test on serial terminal
+
+    '''Blue'''
+    area,cx,cy,code = tt_blue[nb-1]
+    cx = img.width() / 2 - cx
+    cy = img.height() / 2 - cy
+    bAngle = math.pi/2 - math.atan2(cy, cx)
+    bDist = math.sqrt(cx*cx + cy*cy)
+    string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(bAngle)+" | "+str(bDist)+str(area)+"b"
+    #print (string_blue)   # test on serial terminal
+
+    #Now calculate distance and position
+    #Goal 1 is the one in front of the robot
+    #Goal 2 is the one facing the back of the robot
+
+    #convert in [0, 360), to be sure
+    bAngle = int(bAngle * 180 / math.pi)
+    yAngle = int(yAngle * 180 / math.pi)
+
+    bAngle = (bAngle +  360) % 360;
+    yAngle = (yAngle +  360) % 360;
+
+    #Now bring it back to [-179, 180]
+    if bAngle > 180:
+        bAngle = bAngle - 360
+
+    if yAngle > 180:
+        yAngle = yAngle - 360
+
+    if ATTACKING == 1:
+        angle1 = abs(yAngle)
+        angle2 = abs(bAngle - 180)
+    else:
+        angle1 = abs(bAngle)
+        angle2 = abs(yAngle - 180)
+
+    dist1 = (GOALS_DEPTH * math.sin(angle2) ) / (math.sin(angle1+angle2))
+    dist2 = (GOALS_DEPTH * math.sin(angle1) ) / (math.sin(angle1+angle2))
+
+    print("------")
+    print(angle1)
+    print(angle2)
+    print(dist1)
+    print(dist2)
+    print("------")
+
+
+    #print ("..................................")
+
+print(clock.fps())

utility/OpenMV/main_test_conic.py

@@ -29,8 +29,8 @@ blue_led.on()
 #thresholds = [  (54, 93, -10, 25, 55, 70),    # thresholds yellow goal
 #                (30, 45, 1, 40, -60, -19)]    # thresholds blue goal
 #
-thresholds = [  (30, 70, -12, 19, 41, 68)  ,    # thresholds yellow goal
+thresholds = [  (57, 93, -18, 14, 28, 77)  ,    # thresholds yellow goal
-                (0, 70, -2, 34, -59, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (31, 68, -20, 18, -47, -17)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -79,7 +79,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=150, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=70, area_threshold=100, merge = True):
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())