EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

camera: update scripts, line and position handling for H7

pull/1/head
EmaMaker 2021-01-31 18:41:27 +01:00
parent 83bb6f28d5
commit dcdb5ac9d3
11 changed files with 209 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/systems/lines/linesys_2019.h
View File

@ -7,15 +7,6 @@
#include "vars.h" #include "vars.h"
#define S1I 21 //A14 N
#define S1O 20 //A15
#define S2I 17 //A16 E
#define S2O 16 //A17
#define S3I 15 //A20 S
#define S3O 14 //A0
#define S4I 23 //A1 O
#define S4O 22 //A2
#define LINE_THRESH 90 #define LINE_THRESH 90
#define EXTIME 200 #define EXTIME 200
#define LINES_EXIT_SPD 350 #define LINES_EXIT_SPD 350

22
include/systems/lines/linesys_camera.h
View File

@ -7,17 +7,17 @@
#include "vars.h" #include "vars.h"
#define S1I 21 //A14 N #define S1I A7
#define S1O 20 //A15 #define S1O A6
#define S2I 17 //A16 E #define S2I A2
#define S2O 16 //A17 #define S2O A3
#define S3I 15 //A20 S #define S3I A1
#define S3O 14 //A0 #define S3O A0
#define S4I 23 //A1 O #define S4I A9
#define S4O 22 //A2 #define S4O A8
#define LINE_THRESH_CAM 90 #define LINE_THRESH_CAM 300
#define EXIT_TIME 125 #define EXIT_TIME 250
#define LINES_EXIT_SPD 350 #define LINES_EXIT_SPD 350
class LineSysCamera : public LineSystem{ class LineSysCamera : public LineSystem{
@ -36,7 +36,7 @@ class LineSysCamera : public LineSystem{
DataSource* ds; DataSource* ds;
bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i; int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i;
elapsedMillis exitTimer; unsigned long exitTimer;
int outDir, outVel; int outDir, outVel;
byte linesens; byte linesens;
}; };

10
include/systems/position/positionsys_camera.h
View File

@ -5,16 +5,16 @@
//Note: those variables can be changes, and will need to change depending on camera calibration //Note: those variables can be changes, and will need to change depending on camera calibration
//Camera center: those setpoints correspond to the center of the field //Camera center: those setpoints correspond to the center of the field
#define CAMERA_CENTER_X -5 #define CAMERA_CENTER_X -10
#define CAMERA_CENTER_Y -17 #define CAMERA_CENTER_Y 20
//Camera goal: those setpoints correspond to the position of the center of the goal on the field //Camera goal: those setpoints correspond to the position of the center of the goal on the field
#define CAMERA_GOAL_X 0 #define CAMERA_GOAL_X 0
#define CAMERA_GOAL_Y -20 #define CAMERA_GOAL_Y 0
#define CAMERA_CENTER_Y_ABS_SUM 72 #define CAMERA_CENTER_Y_ABS_SUM 50
//Actually it's ± MAX_VAL //Actually it's ± MAX_VAL
#define MAX_X 25 #define MAX_X 50
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2) #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
#define DIST_MULT 1.65 #define DIST_MULT 1.65

7
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -76,8 +76,9 @@ void DataSourceCameraConic ::computeCoordsAngles() {
true_xy = 50 - true_xy; true_xy = 50 - true_xy;
true_yy = true_yy - 50; true_yy = true_yy - 50;
yAngle = 90 - (atan2(true_yy, true_xy) * 180 / 3.14); //-90 + to account for phase shifting with goniometric circle
bAngle = 90 - (atan2(true_yb, true_xb) * 180 / 3.14); yAngle = -90 + (atan2(true_yy, true_xy) * 180 / 3.14);
bAngle = -90 + (atan2(true_yb, true_xb) * 180 / 3.14);
//Now cast angles to [0, 359] domain angle flip them //Now cast angles to [0, 359] domain angle flip them
yAngle = (yAngle + 360) % 360; yAngle = (yAngle + 360) % 360;
bAngle = (bAngle + 360) % 360; bAngle = (bAngle + 360) % 360;
@ -143,7 +144,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
} }
void DataSourceCameraConic::test(){ void DataSourceCameraConic::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu goalOrientation = digitalRead(SWITCH_2); //se HIGH attacco gialla, difendo blu
update(); update();
DEBUG.print("Blue: Angle: "); DEBUG.print("Blue: Angle: ");
DEBUG.print(bAngle); DEBUG.print(bAngle);

158
src/sensors/data_source_camera_vshapedmirror.cpp
View File

@ -1,158 +0,0 @@
#include "behaviour_control/status_vector.h"
#include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h"
DataSourceCameraVShaped::DataSourceCameraVShaped(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
void DataSourceCameraVShaped :: readSensor(){
portx = 999;
while(ser->available() > 0) {
value = ser->read();
// if the incoming character is a 'Y', set the start packet flag
if (value == 'Y') {
startpY = 1;
}
// if the incoming character is a 'Y', set the start packet flag
if (value == 'B') {
startpB = 1;
}
// if the incoming character is a '.', set the end packet flag
if (value == 'y') {
endpY = 1;
}
// if the incoming character is a '.', set the end packet flag
if (value == 'b') {
endpB = 1;
}
if ((startpY == 1) && (endpY == 0)) {
if (isDigit(value)) {
// convert the incoming byte to a char and add it to the string:
valStringY += value;
}else if(value == '-'){
negateY = true;
}
}
if ((startpB == 1) && (endpB == 0)) {
if (isDigit(value)) {
// convert the incoming byte to a char and add it to the string:
valStringB += value;
}else if(value == '-'){
negateB = true;
}
}
if ((startpY == 1) && (endpY == 1)) {
valY = valStringY.toInt(); // valid data
if(negateY) valY *= -1;
valStringY = "";
startpY = 0;
endpY = 0;
negateY = false;
datavalid ++;
}
if ((startpB == 1) && (endpB == 1)) {
valB = valStringB.toInt(); // valid data
if(negateB) valB *= -1;
valStringB = "";
startpB = 0;
endpB = 0;
negateB = false;
datavalid ++;
}
}
}
void DataSourceCameraVShaped :: postProcess(){
if (valY != -74)
oldGoalY = valY;
if (valB != -74)
oldGoalB = valB;
if (valY == -74)
valY = oldGoalY;
if (valB == -74)
valB = oldGoalB;
// entro qui solo se ho ricevuto i pacchetti completi sia del blu che del giallo
if (datavalid > 1 ) {
if(goalOrientation == 1){
//yellow goalpost
pAtk = valY;
pDef = valB * -1;
}else{
//blue goalpost
pAtk = valB;
pDef = valY * -1;
}
//attacco gialla
if(goalOrientation == HIGH){
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;
}
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;
// }
// }
// 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
DEBUG.print(pAtk);
DEBUG.print(" | ");
DEBUG.print(fixCamIMU(pAtk));
DEBUG.print(" --- ");
DEBUG.print(pDef);
DEBUG.print(" | ");
DEBUG.println(fixCamIMU(pDef));
delay(100);
}
int DataSourceCameraVShaped::fixCamIMU(int d){
if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue();
else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360;
imuOff = constrain(imuOff*0.8, -30, 30);
return d + imuOff;
}

10
src/sensors/sensors.cpp
View File

@ -2,14 +2,14 @@
#include "sensors/sensors.h" #include "sensors/sensors.h"
void initSensors(){ void initSensors(){
pinMode(SWITCH_DX, INPUT); pinMode(SWITCH_1, INPUT);
pinMode(SWITCH_SX, INPUT); pinMode(SWITCH_2, INPUT);
pinMode(LED_R, OUTPUT); pinMode(LED_R, OUTPUT);
pinMode(LED_Y, OUTPUT); pinMode(LED_Y, OUTPUT);
pinMode(LED_G, OUTPUT); pinMode(LED_G, OUTPUT);
drive = new DriveController(new Motor(11, 12, 4, 55), new Motor(24, 25, 5, 135), new Motor(26, 27, 2, 225), new Motor(28, 29, 3, 305)); drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
//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)); //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(); compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial2, 57600); ball = new DataSourceBall(&Serial2, 57600);
@ -21,8 +21,8 @@ void initSensors(){
} }
void updateSensors(){ void updateSensors(){
role = digitalRead(SWITCH_DX); role = digitalRead(SWITCH_1);
camera->goalOrientation = digitalRead(SWITCH_SX); camera->goalOrientation = digitalRead(SWITCH_2);
compass->update(); compass->update();
ball->update(); ball->update();

23
src/system/lines/linesys_camera.cpp
View File

@ -2,6 +2,7 @@
#include "systems/position/positionsys_camera.h" #include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "strategy_roles/games.h" #include "strategy_roles/games.h"
#include "behaviour_control/status_vector.h"
LineSysCamera::LineSysCamera(){} LineSysCamera::LineSysCamera(){}
LineSysCamera::LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out_){ LineSysCamera::LineSysCamera(vector<DataSource*> in_, vector<DataSource*> out_){
@ -38,12 +39,12 @@ void LineSysCamera ::update() {
for(auto it = in.begin(); it != in.end(); it++){ for(auto it = in.begin(); it != in.end(); it++){
i = it - in.begin(); i = it - in.begin();
ds = *it; ds = *it;
linetriggerI[i] = ds->getValue() > LINE_THRESH; linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
} }
for(auto it = out.begin(); it != out.end(); it++){ for(auto it = out.begin(); it != out.end(); it++){
i = it - out.begin(); i = it - out.begin();
ds = *it; ds = *it;
linetriggerO[i] = ds->getValue() > LINE_THRESH; linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
} }
for(int i = 0; i < 4; i++){ for(int i = 0; i < 4; i++){
@ -51,21 +52,20 @@ void LineSysCamera ::update() {
outV = outV | (linetriggerO[i] << i); outV = outV | (linetriggerO[i] << i);
} }
if (inV > 0 || outV > 0) {
if ((inV > 0) || (outV > 0)) { if(millis() - exitTimer > EXIT_TIME) {
if(exitTimer > EXIT_TIME) {
fboundsX = true; fboundsX = true;
fboundsY = true; fboundsY = true;
} }
exitTimer = 0; exitTimer = millis();
} }
linesens |= inV | outV; linesens |= inV | outV;
outOfBounds(); outOfBounds();
} }
void LineSysCamera::outOfBounds(){ void LineSysCamera::outOfBounds(){
// digitalWriteFast(BUZZER, LOW);
if(fboundsX == true) { if(fboundsX == true) {
if(linesens & 0x02) linesensOldX = 2; if(linesens & 0x02) linesensOldX = 2;
else if(linesens & 0x08) linesensOldX = 8; else if(linesens & 0x08) linesensOldX = 8;
@ -77,11 +77,12 @@ void LineSysCamera::outOfBounds(){
if(linesensOldY != 0) fboundsY = false; if(linesensOldY != 0) fboundsY = false;
} }
if (exitTimer <= EXTIME){ if (millis() - exitTimer < EXIT_TIME){
((PositionSysCamera*)striker->ps)->goCenter(); CURRENT_DATA_WRITE.game->ps->goCenter();
tookLine = true; tookLine = true;
tone(BUZZER, 220.00, 250);
}else{ }else{
drive->canUnlock = true; // drive->canUnlock = true;
linesens = 0; linesens = 0;
linesensOldY = 0; linesensOldY = 0;
linesensOldX = 0; linesensOldX = 0;

5
src/system/positions/positionsys_camera.cpp
View File

@ -31,7 +31,7 @@ PositionSysCamera::PositionSysCamera() {
} }
void PositionSysCamera::update(){ void PositionSysCamera::update(){
int posx, posy; int posx = 0, posy = 0;
//Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync //Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync
//Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot //Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot
@ -80,6 +80,7 @@ void PositionSysCamera::addMoveOnAxis(int x, int y){
void PositionSysCamera::goCenter(){ void PositionSysCamera::goCenter(){
setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y);
CameraPID();
} }
void PositionSysCamera::centerGoal(){ void PositionSysCamera::centerGoal(){
@ -115,7 +116,7 @@ void PositionSysCamera::CameraPID(){
dir = (dir+360) % 360; dir = (dir+360) % 360;
int dist = sqrt(Outputx*Outputx + Outputy*Outputy); int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350); int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 120);
drive->prepareDrive(dir, speed, 0); drive->prepareDrive(dir, speed, 0);

23
src/test_menu.cpp
View File

@ -12,6 +12,7 @@
#include "strategy_roles/game.h" #include "strategy_roles/game.h"
#include "strategy_roles/games.h" #include "strategy_roles/games.h"
#include "behaviour_control/data_source.h" #include "behaviour_control/data_source.h"
#include "behaviour_control/status_vector.h"
void TestMenu :: testMenu(){ void TestMenu :: testMenu(){
DEBUG.println(); DEBUG.println();
@ -84,27 +85,15 @@ void TestMenu :: testMenu(){
bt->test(); bt->test();
break; break;
case '6': case '6':
updateStatusVector();
camera->test(); camera->test();
delay(100);
break; break;
case '7': case '7':
break;
case '8': case '8':
if(DEBUG.available() == 0){ CURRENT_DATA_READ.game->ls->test();
DEBUG.println("To do Line Sensors test, decide the role first"); delay(200);
DEBUG.println("1)Keeper");
DEBUG.println("2)Goalie");
currentRole = DEBUG.read();
switch(currentRole){
case '1':
(striker->ls)->test();
break;
case '2':
(keeper->ls)->test();
break;
default:
DEBUG.println("INVALID ROLE");
break;
}
}
break; break;
case 'u': case 'u':
while(Serial2.available()) DEBUG.print((char)Serial2.read()); while(Serial2.available()) DEBUG.print((char)Serial2.read());

20
utility/OpenMV/conic_eff.py
View File

@ -38,8 +38,8 @@ blue_led.on()
############################################################################## ##############################################################################
thresholds = [ (71, 100, -24, 12, 57, 99), # thresholds yellow goal thresholds = [ (69, 100, -2, 15, 16, 40), # thresholds yellow goal
(38, 55, -33, -1, 0, 26)] # thresholds blue goal (6, 31, -15, 4, -35, 0) (32, 77, -2, 12, -48, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152) roi = (0, 6, 318, 152)
@ -52,19 +52,19 @@ 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_whitebal(False) # must be turned off for color tracking
sensor.set_auto_exposure(False, 10000) vbc sensor.set_auto_exposure(False, 10000) vbc
#sensor.set_backlight(1) #sensor.set_backlight(1)
#sensor.set_brightness(+2) #sensor.set_brightness(+2 )
#sensor.set_windowing(roi) #sensor.set_windowing(roi)
clock = time.clock()''' clock = time.clock()'''
sensor.reset() sensor.reset()
sensor.set_pixformat(sensor.RGB565) sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) sensor.set_framesize(sensor.QVGA)
sensor.set_contrast(3) sensor.set_contrast(1)
sensor.set_saturation(3) sensor.set_saturation(0)
sensor.set_brightness(0) sensor.set_brightness(3)
sensor.set_quality(0) sensor.set_quality(0)
sensor.set_auto_whitebal(False) sensor.set_auto_whitebal(True)
sensor.set_auto_exposure(False, 5500) sensor.set_auto_exposure(False, 3500)
sensor.set_auto_gain(True) sensor.set_auto_gain(True)
sensor.skip_frames(time = 300) 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 tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata
img = sensor.snapshot() img = sensor.snapshot()
for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=40, merge = True): for blob in img.find_blobs(thresholds, pixels_threshold=40, area_threshold=50, merge = True):
img.draw_rectangle(blob.rect()) img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy()) img.draw_cross(blob.cx(), blob.cy())

153
utility/OpenMV/conic_eff_h7.py Normal file
View File

@ -0,0 +1,153 @@
# 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 = [ (75, 100, -10, 13, 12, 40), # thresholds yellow goal
(40, 70, -13, 13, -35, -11)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
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) vbc
#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.QVGA)
sensor.set_contrast(1)
sensor.set_saturation(0)
sensor.set_brightness(3)
sensor.set_quality(0)
sensor.set_auto_whitebal(True)
sensor.set_auto_exposure(False, 3500)
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=40, area_threshold=50, 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]
#Formulas to compute position of points, considering that the H7 is rotated by a certain angle
#x = y-offset
#y = offset - x
y_cx = int(y1_cy - img.height() / 2)
y_cy = int(img.width() / 2 - y1_cx)
b_cx = int(b1_cy - img.height() / 2)
b_cy = int(img.width() / 2 - b1_cx)
print(str(y_cx) + " | " + str(y_cy) + " --- " + str(b_cx) + " | " + str(b_cy))
#Normalize data between 0 and 100
if y_found == True:
y_cx = val_map(y_cx, -img.height() / 2, img.height() / 2, 100, 0)
y_cy = val_map(y_cy, -img.width() / 2, img.width() / 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.height() / 2, img.height() / 2, 100, 0)
b_cy = val_map(b_cy, -img.width() / 2, img.width() / 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
uart.write(START_BYTE)
uart.write(s_bcx)
uart.write(s_bcy)
uart.write(s_ycx)
uart.write(s_ycy)
uart.write(END_BYTE)