From 74a712121fd5abb79ca4699ca6632cd519c0cd9a Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 2 Mar 2020 15:54:27 +0100 Subject: [PATCH] little correction to the pid for position --- src/position/positionsys_camera.cpp | 6 +- utility/OpenMV/conic_eff.py | 2 +- utility/OpenMV/conic_eff.py.autosave | 149 --------------------------- 3 files changed, 4 insertions(+), 153 deletions(-) delete mode 100644 utility/OpenMV/conic_eff.py.autosave diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp index 17c415e..f417fe0 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/position/positionsys_camera.cpp @@ -52,12 +52,12 @@ void PositionSysCamera :: setCameraPID(){ Outputy = 0; Setpointy = 0; - X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT); + X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); X->SetOutputLimits(-50,50); X->SetMode(AUTOMATIC); X->SetDerivativeLag(1); X->SetSampleTime(2); - Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT); + Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); Y->SetOutputLimits(-50,50); Y->SetMode(AUTOMATIC); Y->SetDerivativeLag(1); @@ -102,7 +102,7 @@ void PositionSysCamera :: CameraPID(){ /*if(abs(Outputx) <= 1 && abs(Outputy) <= 1){ drive->prepareDrive(0,0,0); }else{*/ - int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14); + int dir = -90-(atan2(Outputy,Outputx)*180/3.14); dir = (dir+360) % 360; int dist = sqrt(Outputx*Outputx + Outputy*Outputy); diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index f8e9663..bca8390 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -39,7 +39,7 @@ blue_led.on() thresholds = [ (50, 98, -2, 30, 57, 113), # thresholds yellow goal - (17, 30, -28, -8, -8, 12)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (22, 45, -32, -6, -16, 10)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave deleted file mode 100644 index c91d697..0000000 --- a/utility/OpenMV/conic_eff.py.autosave +++ /dev/null @@ -1,149 +0,0 @@ -# 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 = [ (21, 100, -53, 53, 59, 101), # thresholds yellow goal - (29, 50, -48, 35, -56, 5)] # 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(2) -sensor.set_saturation(1) -sensor.set_brightness(1) -sensor.set_quality(3) -sensor.set_auto_whitebal(False) -sensor.set_auto_exposure(False, 4500) -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=60, area_threshold=90, 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) -