From dcdb5ac9d30f149d83bbf33adec5afda43cf3449 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sun, 31 Jan 2021 18:41:27 +0100 Subject: [PATCH] camera: update scripts, line and position handling for H7 --- include/systems/lines/linesys_2019.h | 9 - include/systems/lines/linesys_camera.h | 22 +-- include/systems/position/positionsys_camera.h | 10 +- .../data_source_camera_conicmirror.cpp | 7 +- .../data_source_camera_vshapedmirror.cpp | 158 ------------------ src/sensors/sensors.cpp | 10 +- src/system/lines/linesys_camera.cpp | 23 +-- src/system/positions/positionsys_camera.cpp | 5 +- src/test_menu.cpp | 23 +-- utility/OpenMV/conic_eff.py | 20 +-- utility/OpenMV/conic_eff_h7.py | 153 +++++++++++++++++ 11 files changed, 209 insertions(+), 231 deletions(-) delete mode 100644 src/sensors/data_source_camera_vshapedmirror.cpp create mode 100644 utility/OpenMV/conic_eff_h7.py diff --git a/include/systems/lines/linesys_2019.h b/include/systems/lines/linesys_2019.h index 355c5d5..58ce5dd 100644 --- a/include/systems/lines/linesys_2019.h +++ b/include/systems/lines/linesys_2019.h @@ -7,15 +7,6 @@ #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 EXTIME 200 #define LINES_EXIT_SPD 350 diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index f0fd759..d0ba408 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -7,17 +7,17 @@ #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 S1I A7 +#define S1O A6 +#define S2I A2 +#define S2O A3 +#define S3I A1 +#define S3O A0 +#define S4I A9 +#define S4O A8 -#define LINE_THRESH_CAM 90 -#define EXIT_TIME 125 +#define LINE_THRESH_CAM 300 +#define EXIT_TIME 250 #define LINES_EXIT_SPD 350 class LineSysCamera : public LineSystem{ @@ -36,7 +36,7 @@ class LineSysCamera : public LineSystem{ DataSource* ds; bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i; - elapsedMillis exitTimer; + unsigned long exitTimer; int outDir, outVel; byte linesens; }; \ No newline at end of file diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 2bde499..80de44f 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -5,16 +5,16 @@ //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 -#define CAMERA_CENTER_X -5 -#define CAMERA_CENTER_Y -17 +#define CAMERA_CENTER_X -10 +#define CAMERA_CENTER_Y 20 //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_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 -#define MAX_X 25 +#define MAX_X 50 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2) #define DIST_MULT 1.65 diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index 36f4e18..7a593bc 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -76,8 +76,9 @@ void DataSourceCameraConic ::computeCoordsAngles() { true_xy = 50 - true_xy; true_yy = true_yy - 50; - yAngle = 90 - (atan2(true_yy, true_xy) * 180 / 3.14); - bAngle = 90 - (atan2(true_yb, true_xb) * 180 / 3.14); + //-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); //Now cast angles to [0, 359] domain angle flip them yAngle = (yAngle + 360) % 360; bAngle = (bAngle + 360) % 360; @@ -143,7 +144,7 @@ void DataSourceCameraConic ::computeCoordsAngles() { } void DataSourceCameraConic::test(){ - goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu + goalOrientation = digitalRead(SWITCH_2); //se HIGH attacco gialla, difendo blu update(); DEBUG.print("Blue: Angle: "); DEBUG.print(bAngle); diff --git a/src/sensors/data_source_camera_vshapedmirror.cpp b/src/sensors/data_source_camera_vshapedmirror.cpp deleted file mode 100644 index c86c88c..0000000 --- a/src/sensors/data_source_camera_vshapedmirror.cpp +++ /dev/null @@ -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; -} \ No newline at end of file diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index 88a52a0..2deba21 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -2,14 +2,14 @@ #include "sensors/sensors.h" void initSensors(){ - pinMode(SWITCH_DX, INPUT); - pinMode(SWITCH_SX, INPUT); + pinMode(SWITCH_1, INPUT); + pinMode(SWITCH_2, INPUT); pinMode(LED_R, OUTPUT); pinMode(LED_Y, 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)); compass = new DataSourceBNO055(); ball = new DataSourceBall(&Serial2, 57600); @@ -21,8 +21,8 @@ void initSensors(){ } void updateSensors(){ - role = digitalRead(SWITCH_DX); - camera->goalOrientation = digitalRead(SWITCH_SX); + role = digitalRead(SWITCH_1); + camera->goalOrientation = digitalRead(SWITCH_2); compass->update(); ball->update(); diff --git a/src/system/lines/linesys_camera.cpp b/src/system/lines/linesys_camera.cpp index 5117329..a28bdbd 100644 --- a/src/system/lines/linesys_camera.cpp +++ b/src/system/lines/linesys_camera.cpp @@ -2,6 +2,7 @@ #include "systems/position/positionsys_camera.h" #include "sensors/sensors.h" #include "strategy_roles/games.h" +#include "behaviour_control/status_vector.h" LineSysCamera::LineSysCamera(){} LineSysCamera::LineSysCamera(vector in_, vector out_){ @@ -38,12 +39,12 @@ void LineSysCamera ::update() { for(auto it = in.begin(); it != in.end(); it++){ i = it - in.begin(); ds = *it; - linetriggerI[i] = ds->getValue() > LINE_THRESH; + linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; } for(auto it = out.begin(); it != out.end(); it++){ i = it - out.begin(); ds = *it; - linetriggerO[i] = ds->getValue() > LINE_THRESH; + linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; } for(int i = 0; i < 4; i++){ @@ -51,21 +52,20 @@ void LineSysCamera ::update() { outV = outV | (linetriggerO[i] << i); } - - if ((inV > 0) || (outV > 0)) { - if(exitTimer > EXIT_TIME) { + if (inV > 0 || outV > 0) { + if(millis() - exitTimer > EXIT_TIME) { fboundsX = true; fboundsY = true; } - exitTimer = 0; + exitTimer = millis(); } linesens |= inV | outV; outOfBounds(); - } +} void LineSysCamera::outOfBounds(){ - + // digitalWriteFast(BUZZER, LOW); if(fboundsX == true) { if(linesens & 0x02) linesensOldX = 2; else if(linesens & 0x08) linesensOldX = 8; @@ -77,11 +77,12 @@ void LineSysCamera::outOfBounds(){ if(linesensOldY != 0) fboundsY = false; } - if (exitTimer <= EXTIME){ - ((PositionSysCamera*)striker->ps)->goCenter(); + if (millis() - exitTimer < EXIT_TIME){ + CURRENT_DATA_WRITE.game->ps->goCenter(); tookLine = true; + tone(BUZZER, 220.00, 250); }else{ - drive->canUnlock = true; + // drive->canUnlock = true; linesens = 0; linesensOldY = 0; linesensOldX = 0; diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 52a074d..beab2de 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -31,7 +31,7 @@ PositionSysCamera::PositionSysCamera() { } 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 //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(){ setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); + CameraPID(); } void PositionSysCamera::centerGoal(){ @@ -115,7 +116,7 @@ void PositionSysCamera::CameraPID(){ dir = (dir+360) % 360; 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); diff --git a/src/test_menu.cpp b/src/test_menu.cpp index 1800a3b..d6edd67 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -12,6 +12,7 @@ #include "strategy_roles/game.h" #include "strategy_roles/games.h" #include "behaviour_control/data_source.h" +#include "behaviour_control/status_vector.h" void TestMenu :: testMenu(){ DEBUG.println(); @@ -84,27 +85,15 @@ void TestMenu :: testMenu(){ bt->test(); break; case '6': + updateStatusVector(); camera->test(); + delay(100); break; case '7': + break; case '8': - if(DEBUG.available() == 0){ - DEBUG.println("To do Line Sensors test, decide the role first"); - 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; - } - } + CURRENT_DATA_READ.game->ls->test(); + delay(200); break; case 'u': while(Serial2.available()) DEBUG.print((char)Serial2.read()); diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 2db0126..a48f36c 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (71, 100, -24, 12, 57, 99), # thresholds yellow goal - (38, 55, -33, -1, 0, 26)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (69, 100, -2, 15, 16, 40), # thresholds yellow goal + (32, 77, -2, 12, -48, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0) 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_exposure(False, 10000) vbc #sensor.set_backlight(1) -#sensor.set_brightness(+2) +#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(3) -sensor.set_brightness(0) +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(False) -sensor.set_auto_exposure(False, 5500) +sensor.set_auto_whitebal(True) +sensor.set_auto_exposure(False, 3500) 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=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_cross(blob.cx(), blob.cy()) diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py new file mode 100644 index 0000000..de5c730 --- /dev/null +++ b/utility/OpenMV/conic_eff_h7.py @@ -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)