EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

code_midgen
EmaMaker 2020-02-18 09:37:36 +01:00
parent 0d5ce9576c
commit f2903c2c6b
18 changed files with 158 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.vscode/extensions.json vendored
View File

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

1
include/data_source.h
View File

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

2
include/data_source_bno055.h
View File

@ -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

8
include/data_source_camera_conicmirror.h
View File

@ -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 getValueDef(bool);
// int getValueAtk(bool);
// int getValueDef(bool);
int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist;

4
include/data_source_camera_vshapedmirror.h
View File

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

9
include/status_vector.h
View File

@ -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,
speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role;
int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix,
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;

1
src/data_source_ball.cpp
View File

@ -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;

1
src/data_source_bno055.cpp
View File

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

29
src/data_source_camera_conicmirror.cpp
View File

@ -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){
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;
}
// int DataSourceCameraConic::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

58
src/data_source_camera_vshapedmirror.cpp
View File

@ -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);
return valY;
}
//attacco blu
if(goalOrientation == LOW){
if(fixed) return fixCamIMU(valB);
return valB;
CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valY);
CURRENT_DATA_WRITE.angleAtk = valY;
CURRENT_DATA_WRITE.angleDef = fixCamIMU(valB);
CURRENT_DATA_WRITE.angleDefFix = valB;
}else{
CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valB);
CURRENT_DATA_WRITE.angleAtkFix = valB;
CURRENT_DATA_WRITE.angleDef = fixCamIMU(valY);
CURRENT_DATA_WRITE.angleDefFix = valY;
}
}
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;
}
}
// int DataSourceCameraVShaped::getValueAtk(bool fixed){
// //attacco gialla
// if(goalOrientation == HIGH){
// if(fixed) return fixCamIMU(valY);
// return valY;
// }
// //attacco blu
// if(goalOrientation == LOW){
// if(fixed) return fixCamIMU(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

55
src/data_source_us.cpp
View File

@ -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
i2c->beginTransmission(i2CAddr);
// sets register pointer to echo 1 register(0x02)
i2c->write(byte(0x02));
i2c->endTransmission();
// transmit to device #112s
i2c->beginTransmission(i2CAddr);
// sets register pointer to echo 1 register(0x02)
i2c->write(byte(0x02));
i2c->endTransmission();
// step 4: request reading from sensor
// request 2 bytes from slave device #112
i2c->requestFrom(i2CAddr, 2);
// step 4: request reading from sensor
// request 2 bytes from slave device #112
i2c->requestFrom(i2CAddr, 2);
// step 5: receive reading from sensor
// receive high byte (overwrites previous reading)
reading = i2c->read();
// shift high byte to be high 8 bits
reading = reading << 8;
// receive low byte as lower 8 bit
reading |= i2c->read();
value = reading;
}
// step 5: receive reading from sensor
// receive high byte (overwrites previous reading)
reading = i2c->read();
// shift high byte to be high 8 bits
reading = reading << 8;
// receive low byte as lower 8 bit
reading |= i2c->read();
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;
}
}

10
src/drivecontroller.cpp
View File

@ -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;
}

7
src/game.cpp
View File

@ -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;
}
}

5
src/goalie.cpp
View File

@ -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;
else if (camera->getValueAtk(true) <= 350 && camera->getValueAtk(true) >= 270) cstorc-=9;
if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) 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);
}

11
src/keeper.cpp
View File

@ -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);
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);
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 && 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){

29
src/positionsys_zone.cpp
View File

@ -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
Ly_mis = usCtrl->getValue(0) + usCtrl->getValue(2) + robot; // lunghezza totale stimata
Lx_mis = CURRENT_DATA_READ.USdx + CURRENT_DATA_READ.USsx + robot; // larghezza 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){

2
utility/OpenMV/conic_eff.py
View File

@ -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)

149
utility/OpenMV/conic_eff.py.autosave
View File

@ -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)