diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..5d8be66 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "lib/Arduino-PID-Library"] + path = lib/Arduino-PID-Library + url = https://github.com/br3ttb/Arduino-PID-Library/ diff --git a/include/drivecontroller.h b/include/drivecontroller.h index 0336a1a..6b5c4d3 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -2,11 +2,12 @@ #include #include "motor.h" +#include "PID_v1.h" //PID Constants -#define KP 2.1 -#define KI 0 -#define KD 0.05 +#define KP 1.2 +#define KI 0.0 +#define KD 0.25 #define UNLOCK_THRESH 800 @@ -32,8 +33,11 @@ class DriveController{ Motor* m2; Motor* m3; Motor* m4; + PID* pid; int pDir, pSpeed, pTilt; - float speed1, speed2, speed3, speed4, errorePre, integral, pidfactor, errorP, errorD, errorI, delta; + int gDir, gSpeed, gTilt; + int speed1, speed2, speed3, speed4, errorePre, integral, pidfactor, errorP, errorD, errorI, delta; + double input, output, setpoint; int vx, vy; float sins[360], cosins[360]; 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 3a25e2c..1f5be34 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -15,6 +15,9 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) pDir = 0; pSpeed = 0; pTilt = 0; + gDir = 0; + gSpeed = 0; + gTilt = 0; vx = 0; vy = 0; @@ -24,14 +27,15 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) speed3 = 0; speed4 = 0; + pid = new PID(&input, &output, &setpoint, (double)KP, (double)KI, (double)KD, P_ON_M, REVERSE); delta = 0; - errorP = 0; - errorI = 0; - errorD = 0; - - errorePre = 0; - pidfactor = 0; - integral = 0; + input = 0; + output = 0; + setpoint = 0; + + pid->SetMode(AUTOMATIC); + pid->SetSampleTime(2); + canUnlock = true; unlockTime = 0; @@ -50,6 +54,7 @@ void DriveController::prepareDrive(int dir, int speed, int tilt){ void DriveController::prepareDrive(int dir, int speed){ pDir = dir; pSpeed = speed; + pDir = gTilt; } void DriveController::drivePrepared(){ @@ -61,6 +66,10 @@ float DriveController::torad(float f){ } void DriveController::drive(int dir, int speed, int tilt){ + gDir = dir; + gSpeed = speed; + gTilt = tilt; + vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); @@ -80,22 +89,21 @@ void DriveController::drive(int dir, int speed, int tilt){ speed4 = -(speed2); // calcola l'errore di posizione rispetto allo 0 - delta = compass->getValue(); - if(delta > 180) delta = delta-360; - delta = delta - tilt; + delta = (compass->getValue()-tilt+360)%360; +; + setpoint = 0; + pid->SetControllerDirection(REVERSE); - // calcola correzione proporzionale - errorP = KP * delta; + if(delta > 180) { + setpoint = 359;//delta = delta-360; + pid->SetControllerDirection(DIRECT); + } - // calcola correzione derivativa - errorD = KD * (delta - errorePre); - errorePre = delta; + input = delta; - // calcola correzione integrativa - integral = 0.5 * integral + delta; - errorI = KI * integral; - // calcota correzione complessiva - pidfactor = errorD + errorP + errorI; + pid->Compute(); + + pidfactor = delta > 180 ? output*-1 : output; speed1 += pidfactor; speed2 += pidfactor; diff --git a/utility/OpenMV/main_test_conic.py.autosave b/utility/OpenMV/main_test_conic.py.autosave new file mode 100644 index 0000000..f2bfe21 --- /dev/null +++ b/utility/OpenMV/main_test_conic.py.autosave @@ -0,0 +1,117 @@ +# 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) + + +# 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, 100, 15, 127, 15, 127), # generic_red_thresholds +# (30, 100, -64, -8, -32, 32), # generic_green_thresholds +# (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds + +#thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal +# (30, 45, 1, 40, -60, -19)] # thresholds blue goal +# +thresholds = [ (30, 70, -12, 19, 41, 68) , # thresholds yellow goal + (0, 70, -2, 34, -59, -21)] # 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(+2) +sensor.set_saturation(+1) +sensor.set_brightness(-3) +sensor.set_quality(0) +sensor.set_auto_exposure(False, 6000) +sensor.set_auto_gain(True) +sensor.skip_frames(time = 300) + +clock = time.clock() +############################################################################## + + + +# [] list +# () tupla + +'''while(True): + clock.tick() + img = sensor.snapshot()''' + +while(True): + clock.tick() + + blue_led.off() + + 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=100, area_threshold=150, 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() ) ] + if (blob.code() == 2): + tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] + + 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) + + '''Yellow''' + area,cx,cy,code = tt_yellow[ny-1] # coordinata x del piu' grande y se montata al contrario + cx = img.width() / 2 - cx + cy = img.height() / 2 - cy + angle = math.pi/2 - math.atan2(cy, cx) + dist = math.sqrt(cx*cx + cy*cy) + string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(angle)+" | "+str(dist)+str(area)+"y" + print (string_yellow) # test on serial terminal + + '''Blue''' + area,cx,cy,code = tt_blue[nb-1] # coordinata x del piu' grande y se montata al contrario + cx = img.width() / 2 - cx + cy = img.height() / 2 - cy + angle = math.pi/2 - math.atan2(cy, cx) + dist = math.sqrt(cx*cx + cy*cy) + string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(angle)+" | "+str(dist)+str(area)+"b" + print (string_blue) # test on serial terminal + + #print ("..................................") + +print(clock.fps())