From 3c34a7167684d8b67ae4e0dd38d7d080c12bb26d Mon Sep 17 00:00:00 2001 From: u-siri-ous Date: Wed, 26 Feb 2020 18:51:46 +0100 Subject: [PATCH] camera pid testing --- include/positionsys_camera.h | 17 ++- lib/Arduino-PID-Library | 1 + src/drivecontroller.cpp | 2 +- src/main.cpp | 1 + src/positionsys_camera.cpp | 72 ++++++++++--- utility/OpenMV/conic_eff.py | 11 +- utility/OpenMV/conic_eff.py.autosave | 149 +++++++++++++++++++++++++++ 7 files changed, 233 insertions(+), 20 deletions(-) create mode 160000 lib/Arduino-PID-Library create mode 100644 utility/OpenMV/conic_eff.py.autosave diff --git a/include/positionsys_camera.h b/include/positionsys_camera.h index 59dce44..7878308 100644 --- a/include/positionsys_camera.h +++ b/include/positionsys_camera.h @@ -1,7 +1,15 @@ +#include "PID_v2.h" #include "systems.h" #define CAMERA_CENTER_X 3 -#define CAMERA_CENTER_Y 6 +#define CAMERA_CENTER_Y 3 + +#define Kpx 1 +#define Kix 0 +#define Kdx 0 +#define Kpy 1 +#define Kiy 0 +#define Kdy 0 class PositionSysCamera : public PositionSystem{ @@ -10,5 +18,12 @@ class PositionSysCamera : public PositionSystem{ void goCenter(); void update() override; void test() override; + void setCameraPID(); + void CameraPID(); + + double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; + + PID* X; + PID* Y; }; diff --git a/lib/Arduino-PID-Library b/lib/Arduino-PID-Library new file mode 160000 index 0000000..9b4ca0e --- /dev/null +++ b/lib/Arduino-PID-Library @@ -0,0 +1 @@ +Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index 71f28ee..9a38054 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -47,7 +47,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) vyn = 0; } -void DriveController::prepareDrive(int dir, int speed, int tilt=0){ +void DriveController::prepareDrive(int dir, int speed, int tilt){ pDir = dir; pSpeed = speed; pTilt = tilt; diff --git a/src/main.cpp b/src/main.cpp index ca69d9f..077a193 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -25,6 +25,7 @@ void loop() { // Last thing to do: movement and update status vector // drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix); + drive->drivePrepared(); updateStatusVector(); } diff --git a/src/positionsys_camera.cpp b/src/positionsys_camera.cpp index 765e0c7..73bbe13 100644 --- a/src/positionsys_camera.cpp +++ b/src/positionsys_camera.cpp @@ -3,7 +3,9 @@ #include "vars.h" #include "sensors.h" -PositionSysCamera::PositionSysCamera() {} +PositionSysCamera::PositionSysCamera() { + setCameraPID(); +} void PositionSysCamera::update(){ } @@ -14,26 +16,70 @@ void PositionSysCamera::test(){ void PositionSysCamera::goCenter(){ /*WORKS BUT CAN BE BETTER*/ //Y - if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); + /* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); //X else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); - else drive->prepareDrive(0, 0, 0); + else drive->prepareDrive(0, 0, 0); */ /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/ - // int x = 1; - // int y = 1; + /* int x = 1; + int y = 1; - // //Trying using an angle - // if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) - // y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; - // if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb; - // if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy; + //Trying using an angle + if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ + if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) + y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; + if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb; + if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy; - // int dir = -90-(atan2(y*1.5,x)*180/3.14); - // dir = (dir+360) % 360; - // drive->prepareDrive(dir, 100, 0); + int dir = -90-(atan2(y*1.5,x)*180/3.14); + dir = (dir+360) % 360; + drive->prepareDrive(dir, 100, 0); + } */ + CameraPID(); +} +//using a pid controller for the movement, or trying at least +void PositionSysCamera :: setCameraPID(){ + X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); + X->SetOutputLimits(-50,50); + X->SetMode(AUTOMATIC); + X->SetDerivativeLag(1); + X->SetControllerDirection(DIRECT); + X->SetSampleTime(1.5); + Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); + Y->SetOutputLimits(-50,50); + Y->SetMode(AUTOMATIC); + Y->SetDerivativeLag(1); + Y->SetControllerDirection(DIRECT); + Y->SetSampleTime(1.5); +} + +void PositionSysCamera :: CameraPID(){ + if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ + Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2; + Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; + Setpointx = 0; + Setpointy = 0; + } + if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){ + Inputx = CURRENT_DATA_READ.cam_xb; + Inputy = CURRENT_DATA_READ.cam_yb; + } + if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){ + Inputx = CURRENT_DATA_READ.cam_xy; + Inputy = CURRENT_DATA_READ.cam_yy; + //Setpointy todo + } + //TODO: no goal seen + X->Compute(); + Y->Compute(); + + int dir = -90-(atan2(Outputy*1.5,Outputx)*180/3.14); + dir = (dir+360) % 360; + if(Outputx > 0) drive->prepareDrive(90, 100, 0); + else drive->prepareDrive(270, 100, 0); } \ No newline at end of file diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 1bd4edf..a7d5194 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (72, 100, -18, 11, 12, 65) , # thresholds yellow goal - (39, 61, -18, 11, -47, -16)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (45, 71, 0, 20, 56, 84) , # thresholds yellow goal + (16, 27, -29, -7, -10, 11)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -60,10 +60,11 @@ 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_saturation(2) +sensor.set_brightness(0) sensor.set_quality(0) -sensor.set_auto_exposure(False, 6000) +sensor.set_auto_whitebal(False) +sensor.set_auto_exposure(False, 2000) sensor.set_auto_gain(True) sensor.skip_frames(time = 300) diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave new file mode 100644 index 0000000..af4518c --- /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 = [ (40, 98, -26, 2, 68, 110) , # thresholds yellow goal + (26, 100, -38, -4, -20, 16)] # 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) 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.QQVGA) +sensor.set_contrast(+3) +sensor.set_saturation(2) +sensor.set_brightness(0) +sensor.set_quality(0) +sensor.set_auto_whitebal(False) +sensor.set_auto_exposure(False, 6000) +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=50, area_threshold=80, 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) +