From f62bb9751f65a3fbd50768b19b95750b7dbc9340 Mon Sep 17 00:00:00 2001 From: u-siri-ous Date: Mon, 17 Feb 2020 17:42:58 +0100 Subject: [PATCH] camera recenter (on x axis) working c: --- .vscode/extensions.json | 12 +-- include/positionsys_zone.h | 2 + src/goalie.cpp | 4 +- src/main.cpp | 2 + src/positionsys_zone.cpp | 11 +- utility/OpenMV/conic_eff.py.autosave | 149 +++++++++++++++++++++++++++ 6 files changed, 167 insertions(+), 13 deletions(-) create mode 100644 utility/OpenMV/conic_eff.py.autosave diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 0f0d740..8281e64 100755 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -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" + ] +} \ No newline at end of file diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h index c7d71d6..945a5ee 100644 --- a/include/positionsys_zone.h +++ b/include/positionsys_zone.h @@ -55,6 +55,8 @@ #define SOUTH_CENTER 8 #define SOUTH_EAST 9 +#define CAMERA_CENTER 3 + class PositionSysZone : public PositionSystem{ public: PositionSysZone(); diff --git a/src/goalie.cpp b/src/goalie.cpp index 275c9a0..a9265a3 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -18,8 +18,8 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goCenter(); - else drive->prepareDrive(0,0,0); + if(ball->ballSeen) this->goalie(45); + else ((PositionSysZone*)ps)->goCenter(); } int dir, degrees2; diff --git a/src/main.cpp b/src/main.cpp index 0459db1..0a39d4d 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,6 +3,7 @@ #include "sensors.h" #include "games.h" #include "status_vector.h" +#include "positionsys_zone.h" void setup() { delay(1500); @@ -23,6 +24,7 @@ void loop() { keeper->play(role==0); // Last thing to do: movement and update status vector + drive->drivePrepared(); updateStatusVector(); } diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index 6cdc079..26dafad 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -373,14 +373,15 @@ void PositionSysZone::testLogicZone(){ void PositionSysZone::goCenter() { - if((camera->true_yb + camera->true_yy) >= 3 && (camera->true_yb + camera->true_yy) <= -3) { + /* if((camera->true_yb + camera->true_yy) >= 3 && (camera->true_yb + camera->true_yy) <= -3) { //dx o sx if((camera->true_yb + camera->true_yy) < 0) drive->prepareDrive(0, 200, 0); else if ((camera->true_yb + camera->true_yy) > 0) drive->prepareDrive(270, 200, 0); - } - if(camera->true_xb >= -10 && camera->true_xb <= 10); - else if(camera->true_xb >= -5 && camera->true_xb <= 35) drive->prepareDrive(90, 200, 0); - else if(camera->true_xb >= 5 && camera->true_xb <= -35) drive->prepareDrive(180, 200, 0); + } */ + + if(camera->true_xb < -CAMERA_CENTER || camera->true_xy < -CAMERA_CENTER) drive->prepareDrive(90, 75, 0); + else if(camera->true_xb > CAMERA_CENTER || camera->true_xy > CAMERA_CENTER) drive->prepareDrive(270, 75, 0); + else drive->prepareDrive(0, 0, 0); /* if (zoneIndex == 8) drive->prepareDrive(330, GOCENTER_VEL); if (zoneIndex == 7) diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave new file mode 100644 index 0000000..01dc8c7 --- /dev/null +++ b/utility/OpenMV/conic_eff.py.autosave @@ -0,0 +1,149 @@ +# 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 = [ (30, 70, -12, 19, 10, 57), # thresholds yellow goal + (0, 44, -5, 42, -65, -13)] # 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) +