Now using status vector everywhere :D (To be tested)

.vscode/extensions.json

@@ -1,7 +1,7 @@
 {
-	// See http://go.microsoft.com/fwlink/?LinkId=827846
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
-	// for the documentation about the extensions.json format
+    // for the documentation about the extensions.json format
-	"recommendations": [
+    "recommendations": [
-		"platformio.platformio-ide"
+        "platformio.platformio-ide"
-	]
+    ]
 }

include/data_source.h

@@ -4,7 +4,6 @@
 #include "Arduino.h"
 #include "HardwareSerial.h"
 #include "vars.h"
-#include "status_vector.h"
 
 class DataSource {
 

include/data_source_bno055.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include "data_source.h"
 #include <Adafruit_BNO055.h>
+#include "data_source.h"
 #include <Arduino.h>
 
 #define DATA_CLOCK 10

include/data_source_camera_conicmirror.h

@@ -1,11 +1,13 @@
 #pragma once
+
+#include "data_source.h"
+
 #define startp 105
 #define endp 115
 //Coords are mapped from 0 up to this value
 #define MAP_MAX 100
 #define HALF_MAP_MAX  50
 //#define unkn 0b01101001 
-#include "data_source.h"
 
 class DataSourceCameraConic : public DataSource{
 
@@ -13,8 +15,8 @@ class DataSourceCameraConic : public DataSource{
         DataSourceCameraConic(HardwareSerial* ser, int baud);
         void test() override;
         void readSensor() override;
-        int getValueAtk(bool);
+        // int getValueAtk(bool);
-        int getValueDef(bool);
+        // int getValueDef(bool);
 
         int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist;
 

include/data_source_camera_vshapedmirror.h

@@ -9,8 +9,8 @@ class DataSourceCameraVShaped : public DataSource{
         void test() override;
         int fixCamIMU(int);
         void readSensor() override;
-        int getValueAtk(bool);
+        // int getValueAtk(bool);
-        int getValueDef(bool);
+        // int getValueDef(bool);
         
         int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
         int cameraReady;

include/status_vector.h

@@ -1,5 +1,7 @@
 #pragma once
 #include <Arduino.h>
+#include "game.h"
+#include "systems.h"
 
 /**
  * STATUS VECTOR:
@@ -33,8 +35,11 @@ typedef struct input{
 }input;
 
 typedef struct data{
-    int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist,
+    int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix,
-        speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role;
+        speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role, axisBlock[4];
+    Game* game;
+    LineSystem* lineSystem;
+    PositionSystem* posSystem;
     byte xb, yb, xy, yy, lineSeen, lineActive;
     bool mate, ATKgoal, DEFgoal, ballSeen;
 }data;

src/data_source_ball.cpp

@@ -1,5 +1,6 @@
 #include "data_source_ball.h"
 #include "vars.h"
+#include "status_vector.h"
 
 DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(ser_, baud) {
   ballSeen = false;

src/data_source_bno055.cpp

@@ -1,4 +1,5 @@
 #include "data_source_bno055.h"
+#include "status_vector.h"
 
 //bool loaded = false;
 

src/data_source_camera_conicmirror.cpp

@@ -1,5 +1,5 @@
 #include "data_source_camera_conicmirror.h"
-#include "sensors.h"
+#include "status_vector.h"
 
 DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
   true_xb = 0;
@@ -78,18 +78,29 @@ void DataSourceCameraConic :: readSensor(){
     CURRENT_DATA_WRITE.bAngleFix = bAngleFix;
     CURRENT_DATA_WRITE.yDist = yDist;
     CURRENT_DATA_WRITE.bDist = bDist;
+    if(goalOrientation == HIGH){
+      CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
+      CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
+      CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
+      CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
+    }else{
+      CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
+      CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
+      CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
+      CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
+    }
   }
 }
 
 
-int DataSourceCameraConic::getValueAtk(bool fixed){
+// int DataSourceCameraConic::getValueAtk(bool fixed){
-  if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
+//   if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
-  else return goalOrientation == HIGH ? yAngle : bAngle;
+//   else return goalOrientation == HIGH ? yAngle : bAngle;
-}
+// }
-int DataSourceCameraConic::getValueDef(bool fixed){
+// int DataSourceCameraConic::getValueDef(bool fixed){
-  if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix;
+//   if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix;
-  else return goalOrientation == LOW ? yAngle : bAngle;
+//   else return goalOrientation == LOW ? yAngle : bAngle;
-}
+// }
 
 void DataSourceCameraConic::test(){
   goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu

src/data_source_camera_vshapedmirror.cpp

@@ -1,5 +1,7 @@
 #include "data_source_camera_vshapedmirror.h"
 #include "sensors.h"
+#include "status_vector.h"
+
 
 DataSourceCameraVShaped::DataSourceCameraVShaped(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
 
@@ -64,7 +66,6 @@ void DataSourceCameraVShaped :: readSensor(){
 }
 
 void DataSourceCameraVShaped :: postProcess(){
-
   if (valY != -74)
     oldGoalY = valY;
   if (valB != -74)
@@ -90,33 +91,46 @@ void DataSourceCameraVShaped :: postProcess(){
     datavalid = 0;
     cameraReady = 1;  //attivo flag di ricezione pacchetto
   }
-}
 
-int DataSourceCameraVShaped::getValueAtk(bool fixed){
   //attacco gialla
   if(goalOrientation == HIGH){
-    if(fixed) return fixCamIMU(valY);
+    CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valY);
-    return valY;
+    CURRENT_DATA_WRITE.angleAtk = valY;
-  }
+    CURRENT_DATA_WRITE.angleDef = fixCamIMU(valB);
-  //attacco blu
+    CURRENT_DATA_WRITE.angleDefFix = valB;
-  if(goalOrientation == LOW){
+  }else{
-    if(fixed) return fixCamIMU(valB);
+    CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valB);
-    return valB;
+    CURRENT_DATA_WRITE.angleAtkFix = valB;
+    CURRENT_DATA_WRITE.angleDef = fixCamIMU(valY);
+    CURRENT_DATA_WRITE.angleDefFix = valY;
   }
 }
 
-int DataSourceCameraVShaped::getValueDef(bool fixed){
+// int DataSourceCameraVShaped::getValueAtk(bool fixed){
-  //difendo gialla
+//   //attacco gialla
-  if(goalOrientation == HIGH){
+//   if(goalOrientation == HIGH){
-    if(fixed) return fixCamIMU(valY);
+//     if(fixed) return fixCamIMU(valY);
-    return valY;
+//     return valY;
-  }
+//   }
-  //difendo blu
+//   //attacco blu
-  if(goalOrientation == LOW){
+//   if(goalOrientation == LOW){
-    if(fixed) return fixCamIMU(valB);
+//     if(fixed) return fixCamIMU(valB);
-    return valB;
+//     return valB;
-  }
+//   }
-}
+// }
+
+// int DataSourceCameraVShaped::getValueDef(bool fixed){
+//   //difendo gialla
+//   if(goalOrientation == HIGH){
+//     if(fixed) return fixCamIMU(valY);
+//     return valY;
+//   }
+//   //difendo blu
+//   if(goalOrientation == LOW){
+//     if(fixed) return fixCamIMU(valB);
+//     return valB;
+//   }
+// }
 
 void DataSourceCameraVShaped::test(){
     goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu

src/data_source_us.cpp

@@ -1,5 +1,7 @@
 #include "data_source_us.h"
 #include "vars.h"
+#include "status_vector.h"
+
 
 DataSourceUS::DataSourceUS(TwoWire* i2c_, int addr) : DataSource(i2c_, addr){
   us_flag = false;
@@ -76,22 +78,41 @@ void DataSourceUS::usTrigger() {
 }
 
 void DataSourceUS::usReceive() {
-    // transmit to device #112s
+  // transmit to device #112s
-    i2c->beginTransmission(i2CAddr);
+  i2c->beginTransmission(i2CAddr);
-    // sets register pointer to echo 1 register(0x02)
+  // sets register pointer to echo 1 register(0x02)
-    i2c->write(byte(0x02));
+  i2c->write(byte(0x02));
-    i2c->endTransmission();
+  i2c->endTransmission();
 
-    // step 4: request reading from sensor
+  // step 4: request reading from sensor
-    // request 2 bytes from slave device #112
+  // request 2 bytes from slave device #112
-    i2c->requestFrom(i2CAddr, 2);
+  i2c->requestFrom(i2CAddr, 2);
 
-    // step 5: receive reading from sensor
+  // step 5: receive reading from sensor
-    // receive high byte (overwrites  previous reading)
+  // receive high byte (overwrites  previous reading)
-    reading = i2c->read();
+  reading = i2c->read();
-    // shift high byte to be high 8 bits
+  // shift high byte to be high 8 bits
-    reading = reading << 8;
+  reading = reading << 8;
-    // receive low byte as lower 8 bit
+  // receive low byte as lower 8 bit
-    reading |= i2c->read();
+  reading |= i2c->read();
-    value = reading;
+  value = reading;
+
+  switch(i2CAddr){
+    case 112:
+    CURRENT_INPUT_WRITE.USfr = value;
+    CURRENT_DATA_WRITE.USfr = value;
+    break;
+    case 113:
+    CURRENT_INPUT_WRITE.USdx = value;
+    CURRENT_DATA_WRITE.USdx = value;
+    break;
+    case 114:
+    CURRENT_INPUT_WRITE.USrr = value;
+    CURRENT_DATA_WRITE.USrr = value;
+    break;
+    case 115:
+    CURRENT_INPUT_WRITE.USsx = value;
+    CURRENT_DATA_WRITE.USsx = value;
+    break;
   }
+}

src/drivecontroller.cpp

@@ -1,5 +1,6 @@
 #include "drivecontroller.h"
 #include "sensors.h"
+#include "status_vector.h"
 
 DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){
     m1 = m1_;
@@ -60,6 +61,7 @@ float DriveController::torad(float f){
 }
 
 void DriveController::drive(int dir, int speed, int tilt){
+
     vx = ((speed * cosins[dir]));
     vy = ((-speed * sins[dir]));
 
@@ -102,4 +104,12 @@ void DriveController::drive(int dir, int speed, int tilt){
     m2->drive((int) speed2);
     m3->drive((int) speed3);
     m4->drive((int) speed4);
+
+    CURRENT_DATA_WRITE.dir = dir;
+    CURRENT_DATA_WRITE.speed = speed;
+    CURRENT_DATA_WRITE.tilt = tilt;
+    CURRENT_DATA_WRITE.axisBlock[0] = vxp;
+    CURRENT_DATA_WRITE.axisBlock[1] = vxn;
+    CURRENT_DATA_WRITE.axisBlock[2] = vyp;
+    CURRENT_DATA_WRITE.axisBlock[3] = vyn;
 } 

src/game.cpp

@@ -1,4 +1,5 @@
 #include "game.h"
+#include "status_vector.h"
 
 Game::Game() {}
 Game::Game(LineSystem* ls_, PositionSystem* ps_) {
@@ -11,6 +12,10 @@ void Game::play(bool condition){
     if(condition) {
         realPlay();
         ls->update();
-    }
 
+        CURRENT_DATA_WRITE.posSystem = (this->ps);
+        CURRENT_DATA_WRITE.lineSystem = (this->ls);
+        CURRENT_DATA_WRITE.game = this;
+
+    }
 }

src/goalie.cpp

@@ -1,6 +1,7 @@
 #include "goalie.h"
 #include "sensors.h"
 #include "vars.h"
+#include "status_vector.h"
 #include "positionsys_zone.h"
 
 Goalie::Goalie() : Game() {
@@ -46,8 +47,8 @@ void Goalie::goalie(int plusang) {
 }
 
 void Goalie::storcimentoPorta() {
-  if (camera->getValueAtk(true ) >= 10 && camera->getValueAtk(true) <= 90) cstorc+=9;
+  if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9;
-  else if (camera->getValueAtk(true)  <= 350 && camera->getValueAtk(true) >= 270) cstorc-=9;
+  else if (CURRENT_DATA_READ.angleAtkFix  <= 350 && CURRENT_DATA_READ.angleAtkFix >= 270) cstorc-=9;
   // else cstorc *= 0.7;
   cstorc = constrain(cstorc, -45, 45);
 }

src/keeper.cpp

@@ -3,6 +3,7 @@
 #include "games.h"
 #include "linesys_2019.h"
 #include <Arduino.h>
+#include "status_vector.h"
 
 Keeper::Keeper() : Game() {
     init();
@@ -44,8 +45,8 @@ void Keeper::keeper() {
         angle = (KEEPER_ANGLE_DX + ball->angle) * M_PI / 180;
         angleX = abs(cos(angle));
 
-        if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && camera->getValueDef(true) < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
+        if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && CURRENT_DATA_READ.angleDefFix < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
-        else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360  && camera->getValueDef(true) > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
+        else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360  && CURRENT_DATA_READ.angleDefFix > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT);
         else if(ball->angle < KEEPER_ANGLE_SX && ball->angle > KEEPER_ANGLE_DX){
             int ball_degrees2 = ball->angle > 180? ball->angle-360:ball->angle;
             int dir = ball_degrees2 > 0 ? ball->angle + KEEPER_BALL_BACK_ANGLE : ball->angle - KEEPER_BALL_BACK_ANGLE;
@@ -57,11 +58,11 @@ void Keeper::keeper() {
 }
 
 void Keeper::centerGoalPostCamera(bool checkBack){
-    if (camera->getValueDef(true) > CENTERGOALPOST_CAM_MAX) {
+    if (CURRENT_DATA_READ.angleDefFix > CENTERGOALPOST_CAM_MAX) {
         drive->prepareDrive(KEEPER_ANGLE_SX, CENTERGOALPOST_VEL1);
-    } else if (camera->getValueDef(true) < CENTERGOALPOST_CAM_MIN) {
+    } else if (CURRENT_DATA_READ.angleDefFix < CENTERGOALPOST_CAM_MIN) {
         drive->prepareDrive(KEEPER_ANGLE_DX, CENTERGOALPOST_VEL1);
-    }else if(camera->getValueDef(true) > CENTERGOALPOST_CAM_MIN && camera->getValueDef(true) < CENTERGOALPOST_CAM_MAX){
+    }else if(CURRENT_DATA_READ.angleDefFix > CENTERGOALPOST_CAM_MIN && CURRENT_DATA_READ.angleDefFix < CENTERGOALPOST_CAM_MAX){
         if(!ball->ballSeen) drive->prepareDrive(0, 0, 0);
         if(checkBack){
             if(usCtrl->getValue(2) > CENTERGOALPOST_US_MAX){

src/positionsys_zone.cpp

@@ -1,6 +1,7 @@
 #include "positionsys_zone.h"
 #include "vars.h"
 #include "sensors.h"
+#include "status_vector.h"
 
 PositionSysZone::PositionSysZone(){
     for(int i = 0; i < 3; i++){
@@ -174,17 +175,17 @@ void PositionSysZone::phyZoneUS(){
   else
     DxF = DxF_Def;
 
-  Lx_mis = usCtrl->getValue(1) + usCtrl->getValue(3) + robot; // larghezza totale stimata
+  Lx_mis = CURRENT_DATA_READ.USdx + CURRENT_DATA_READ.USsx + robot; // larghezza totale stimata
-  Ly_mis = usCtrl->getValue(0) + usCtrl->getValue(2) + robot; // lunghezza totale stimata
+  Ly_mis = CURRENT_DATA_READ.USfr + CURRENT_DATA_READ.USrr + robot; // lunghezza totale stimata
 
   // controllo orizzontale
-  if ((Lx_mis < Lx_max) && (Lx_mis > Lx_min) && (usCtrl->getValue(1) > 25) && (usCtrl->getValue(3) > 25)) {
+  if ((Lx_mis < Lx_max) && (Lx_mis > Lx_min) && (CURRENT_DATA_READ.USdx > 25) && (CURRENT_DATA_READ.USsx > 25)) {
     // se la misura orizzontale é accettabile
     good_field_x = true;
     status_x = CENTER;
-    if (usCtrl->getValue(1) < DxF) // robot é vicino al bordo dEASTro
+    if (CURRENT_DATA_READ.USdx < DxF) // robot é vicino al bordo dEASTro
       status_x = EAST;
-    if (usCtrl->getValue(3) < DxF) // robot é vicino al bordo sinistro
+    if (CURRENT_DATA_READ.USsx < DxF) // robot é vicino al bordo sinistro
       status_x = WEST;
 
     if (status_x == CENTER) {
@@ -200,7 +201,7 @@ void PositionSysZone::phyZoneUS(){
     }
   } else {
     // la misura non é pulita per la presenza di un ostacolo
-    if ((usCtrl->getValue(1) >= (DxF + 10)) || (usCtrl->getValue(3) >= (DxF + 10))) {
+    if ((CURRENT_DATA_READ.USdx >= (DxF + 10)) || (CURRENT_DATA_READ.USsx >= (DxF + 10))) {
       // se ho abbastanza spazio a dEASTra o a sinistra
       // devo stare per forza al cento
       status_x = CENTER;
@@ -233,19 +234,19 @@ void PositionSysZone::phyZoneUS(){
     // se la misura verticale é accettabile
     good_field_y = true;
     status_y = CENTER;
-    if (usCtrl->getValue(0) < Dy) {
+    if (CURRENT_DATA_READ.USfr < Dy) {
       status_y = NORTH; // robot é vicino alla porta avversaria
       if (Dy == DyP)
         goal_zone = true; //  davanti alla porta in zona goal
     }
-    if (usCtrl->getValue(2) < Dy)
+    if (CURRENT_DATA_READ.USrr < Dy)
       status_y = SOUTH; // robot é vicino alla propria porta
   } else {
     // la misura non é pulita per la presenza di un ostacolo
     status_y = 255; // non so la coordinata y
-    if (usCtrl->getValue(0) >= (Dy + 0))
+    if (CURRENT_DATA_READ.USfr >= (Dy + 0))
       status_y = CENTER; // ma se ho abbastanza spazio dietro o avanti
-    if (usCtrl->getValue(2) >= (Dy + 0))
+    if (CURRENT_DATA_READ.USrr >= (Dy + 0))
       status_y = CENTER; //  e'probabile che stia al CENTER
   }
 
@@ -262,9 +263,9 @@ void PositionSysZone::phyZoneUS(){
 void PositionSysZone::phyZoneCam(){
 
   //IMU-fixed attack angle
-  camA = camera->getValueAtk(true);
+  camA = CURRENT_DATA_READ.angleAtkFix;
   //IMU-fixed defence angle
-  camD = camera->getValueDef(true);
+  camD = CURRENT_DATA_READ.angleDefFix;
 
   //Negative angle means that the robot is positioned to the right of the goalpost
   //Positive angle means that the robot is positioned to the left of the goalpost
@@ -416,7 +417,7 @@ void PositionSysZone::goCenter() {
 //     drive->prepareDrive(270, vel);
 //   } else {
 //     stop_menamoli = false;
-//     if (usCtrl->getValue(2) >= 25)
+//     if (CURRENT_DATA_READ.USrr >= 25)
 //       drive->prepareDrive(180, vel);
 //     else
 //       drive->prepareDrive(0, 0);
@@ -425,7 +426,7 @@ void PositionSysZone::goCenter() {
 
 
 void PositionSysZone::AAANGOLO() {
-  if((usCtrl->getValue(2) <= 45) && ((usCtrl->getValue(1) <= 50) || (usCtrl->getValue(3) <= 50))) drive->prepareDrive(0, 350, 0);
+  if((CURRENT_DATA_READ.USrr <= 45) && ((CURRENT_DATA_READ.USdx <= 50) || (CURRENT_DATA_READ.USsx <= 50))) drive->prepareDrive(0, 350, 0);
 }
 
 int PositionSysZone::diff(int a, int b){

utility/OpenMV/conic_eff.py

@@ -51,7 +51,7 @@ 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_auto_exposure(False, 10000) vbc
 #sensor.set_backlight(1)
 #sensor.set_brightness(+2)
 #sensor.set_windowing(roi)

utility/OpenMV/conic_eff.py.autosave

@@ -1,149 +0,0 @@
-# color tracking with conic mirror - By: EmaMaker - wed 15 jan 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)
-
-START_BYTE = chr(105) #'i'
-END_BYTE = chr(115) #'s'
-BYTE_UNKNOWN = chr(116) #'t'
-
-y_found = False
-b_found = False
-
-#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
-def val_map(x, in_min, in_max, out_min, out_max):
-    x = int(x)
-    in_min = int(in_min)
-    in_max = int(in_max)
-    out_min = int(out_min)
-    out_max = int(out_max)
-    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
-
-
-# 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 = [  (0, 99, -16, 19, 13, 85),    # thresholds yellow goal
-                (26, 52, -8, 19, -49, -18)]  # 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(+3)
-sensor.set_saturation(0)
-sensor.set_brightness(-2)
-sensor.set_quality(0)
-sensor.set_auto_exposure(False, 10000)
-sensor.set_auto_gain(True)
-sensor.skip_frames(time = 300)
-
-clock = time.clock()
-##############################################################################
-
-
-while(True):
-    clock.tick()
-
-    blue_led.off()
-
-    y_found = False
-    b_found = False
-
-    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=75, area_threshold=130, 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() ) ]
-            y_found = True
-        if (blob.code() == 2):
-            tt_blue = tt_blue +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-            b_found = True
-
-    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)
-
-    y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
-    b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1]
-
-    y_cx = int(img.width() / 2 - y1_cx)
-    y_cy = int(img.height() / 2 - y1_cy)
-    b_cx = int(img.width() / 2 - b1_cx)
-    b_cy = int(img.height() / 2 - b1_cy)
-
-    #Normalize data between 0 and 100
-    if y_found == True:
-        y_cx = val_map(y_cx, -img.width() / 2, img.width() / 2, 100, 0)
-        y_cy = val_map(y_cy, -img.height() / 2, img.height() / 2, 0, 100)
-        #Prepare for send as a list of characters
-        s_ycx = chr(y_cx)
-        s_ycy = chr(y_cy)
-    else:
-        y_cx = BYTE_UNKNOWN
-        y_cy = BYTE_UNKNOWN
-        #Prepare for send as a list of characters
-        s_ycx = y_cx
-        s_ycy = y_cy
-
-    if b_found == True:
-        b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 100, 0)
-        b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
-
-        #Prepare for send as a list of characters
-        s_bcx = chr(b_cx)
-        s_bcy = chr(b_cy)
-    else:
-        b_cx = BYTE_UNKNOWN
-        b_cy = BYTE_UNKNOWN
-        #Prepare for send as a list of characters
-        s_bcx = b_cx
-        s_bcy = b_cy
-
-    print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
-
-
-    uart.write(START_BYTE)
-    uart.write(s_bcx)
-    uart.write(s_bcy)
-    uart.write(s_ycx)
-    uart.write(s_ycy)
-    uart.write(END_BYTE)
-