From 41fefc63c6e32e9df6c0c07a3e3e547f88b066e9 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 14 May 2021 17:53:47 +0200 Subject: [PATCH] latest romecup code --- include/motors_movement/drivecontroller.h | 2 +- .../sensors/data_source_camera_conicmirror.h | 8 +- include/strategy_roles/striker.h | 2 +- src/main.cpp | 6 +- src/strategy_roles/games.cpp | 2 +- src/strategy_roles/precision_shooter.cpp | 6 +- utility/OpenMV/conic_eff_h7.py | 2 +- utility/OpenMV/conic_eff_h7.py.autosave | 219 ++++++++++++++++++ 8 files changed, 236 insertions(+), 11 deletions(-) create mode 100644 utility/OpenMV/conic_eff_h7.py.autosave diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 28679de..c043e57 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -20,7 +20,7 @@ //Max possible vel 310 -#define MAX_VEL 150 +#define MAX_VEL 120 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8) #define MAX_VEL_HALF ((int)MAX_VEL*0.5) #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75) diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 7ad04d8..747ec32 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -21,8 +21,14 @@ /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time. These values need to be subtracted from the coords used in setMoveSetpoints*/ + +// Robot without roller +// #define CAMERA_TRANSLATION_X 0 +// #define CAMERA_TRANSLATION_Y 7 + +//Robot with roller #define CAMERA_TRANSLATION_X 0 -#define CAMERA_TRANSLATION_Y 0 +#define CAMERA_TRANSLATION_Y 7 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index 0853fc0..720aefe 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -6,7 +6,7 @@ #define STRIKER_ATTACK_DISTANCE 110 #define STRIKER_TILT_STOP_DISTANCE 140 -#define STRIKER_PLUSANG 57 +#define STRIKER_PLUSANG 62 #define STRIKER_PLUSANG_VISIONCONE 7 class Striker : public Game{ diff --git a/src/main.cpp b/src/main.cpp index b409ac3..679119b 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -52,10 +52,10 @@ void loop() { drive->resetDrive(); striker_condition = role == HIGH; - // striker->play(striker_condition); + striker->play(1); - if(role) precision_shooter->play(1); - else pass_and_shoot->play(1); + // if(role) precision_shooter->play(1); + // else pass_and_shoot->play(1); // keeper_condition = role == LOW; // keeper->play(keeper_condition); diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index c95ff38..2b6f64c 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -14,6 +14,6 @@ void initGames(){ // striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera()); striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera()); pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera()); - precision_shooter = new PrecisionShooter(new LineSystemEmpty(), new PositionSysCamera()); + precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera()); // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera()); } \ No newline at end of file diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp index 3d7717c..875ed10 100644 --- a/src/strategy_roles/precision_shooter.cpp +++ b/src/strategy_roles/precision_shooter.cpp @@ -59,8 +59,8 @@ void PrecisionShooter::striker(){ dir = (dir + 360) % 360; drive->prepareDrive(dir, MAX_VEL_HALF, tilt()); - if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED); - else roller->speed(roller->MIN); + // if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED); + // else roller->speed(roller->MIN); if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 78 && ( (CURRENT_DATA_READ.posy >= 32 && (CURRENT_DATA_READ.posx >= 15 || CURRENT_DATA_READ.posx <= -15)) || abs(tilt()) > 65 ) ) { // Just let the robot slowly approach the ball @@ -74,7 +74,7 @@ void PrecisionShooter::striker(){ } if(millis() - t3 < 800){ - roller->speed(roller->MAX); + // roller->speed(roller->MAX); drive->prepareDrive(180, MAX_VEL_3QUARTERS, 0); ignited = false; } diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index 1661ce3..a2a2f13 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -46,7 +46,7 @@ blue_led.on() thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz - (34, 54, -23, 13, -61, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (50, 77, -23, 8, -60, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (50, 0, 250, 200) diff --git a/utility/OpenMV/conic_eff_h7.py.autosave b/utility/OpenMV/conic_eff_h7.py.autosave new file mode 100644 index 0000000..b910e83 --- /dev/null +++ b/utility/OpenMV/conic_eff_h7.py.autosave @@ -0,0 +1,219 @@ +# 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) + +# Check side +def isInLeftSide(img, x): + return x < img.width() / 2 + +def isInRightSide(img, x): + return x > img.width() / 2 + + +# 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 = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz + (50, 77, -23, 8, -60, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + + + + +roi = (50, 0, 250, 200) + +# 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_windowing(roi) +sensor.set_contrast(0) +sensor.set_saturation(2) +sensor.set_brightness(3) +sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -1.804)) +sensor.set_auto_exposure(False, 6576) +#sensor.set_auto_gain(False, gain_db=8.78) +sensor.skip_frames(time = 300) + +clock = time.clock() +############################################################################## + + +while(True): + clock.tick() + + print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db())) + + 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=80, area_threshold=100, 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) + + + #Formulas to compute position of points, considering that the H7 is rotated by a certain angle + #x = y-offset + #y = offset - x + + #Compute everything related to Yellow First + + y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1] + + + y_cx = int(y1_cy - img.height() / 2) + y_cy = int(img.width() / 2 - y1_cx) + + + #Normalize data between 0 and 100 + if y_found == True: + img.draw_cross(y1_cx, y1_cy) + + 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 + + + + #Compute everything relative to Blue + '''Given the light situation in our lab and given that blue is usually harder to spot than yellow, we need to check it we got + a blue blob that is in the same side of the ground as the yellow one, if so, discard it and check a new one + ''' + + b_cx = BYTE_UNKNOWN + b_cy = BYTE_UNKNOWN + #Prepare for send as a list of characters + s_bcx = b_cx + s_bcy = b_cy + + if b_found == True: + for i in range(nb-1, 0,-1): + b_area, b1_cx, b1_cy, b_code = tt_blue[i] + if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))): + + img.draw_cross(b1_cx, b1_cy) + + b_cx = int(b1_cy - img.height() / 2) + b_cy = int(img.width() / 2 - b1_cx) + + #print("before :" + str(b_cx) + " " + str(b_cy)) + + 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) + + #print("after :" + str(b_cx) + " " + str(b_cy)) + + #Prepare for send as a list of characters + s_bcx = chr(b_cx) + s_bcy = chr(b_cy) + + '''index = 1 + if b_found == True: + while nb-index >= 0: + b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index] + + index += 1 + # If the two blobs are on opposide side of the field, everything is good + if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))): + + img.draw_cross(b1_cx, b1_cy) + + b_cx = int(b1_cy - img.height() / 2) + b_cy = int(img.width() / 2 - b1_cx) + + print("before :" + str(b_cx) + " " + str(b_cy)) + 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) + + print("after :" + str(b_cx) + " " + str(b_cy)) + + #Prepare for send as a list of characters + s_bcx = chr(b_cx) + s_bcy = chr(b_cy) + + break + 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)