diff --git a/include/goalie.h b/include/goalie.h index 2c03032..0524463 100755 --- a/include/goalie.h +++ b/include/goalie.h @@ -16,7 +16,7 @@ #define GOALIE_ATKDIR_PLUSANG2_COR 70 #define GOALIE_ATKDIR_PLUSANG3_COR 70 -class Goalie : public Game{ +class Goalie : public Game, public PositionSysZone{ public: Goalie(); diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h index 24dedff..c7d71d6 100644 --- a/include/positionsys_zone.h +++ b/include/positionsys_zone.h @@ -60,6 +60,7 @@ class PositionSysZone : public PositionSystem{ PositionSysZone(); void update() override; void test() override; + void goCenter(); //returns the zone calculated int getValue(); @@ -110,7 +111,7 @@ class PositionSysZone : public PositionSystem{ void testLogicZone(); //movement - void goCenter(); + //void goCenter(); void goGoalPost(); void centerGoalPost(); void centerGoalPostCamera(bool); diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index a0a8373..a3117ba 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -31,7 +31,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) setpoint = 0; pid->SetMode(AUTOMATIC); - pid->SetSampleTime(2); + pid->SetSampleTime(5); canUnlock = true; unlockTime = 0; diff --git a/src/goalie.cpp b/src/goalie.cpp index 1787c64..f46220b 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -1,6 +1,7 @@ #include "goalie.h" #include "sensors.h" #include "vars.h" +#include "positionsys_zone.h" Goalie::Goalie() : Game() { init(); @@ -17,7 +18,7 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(50); + if(ball->ballSeen) this->goCenter(); else drive->prepareDrive(0,0,0); } @@ -36,11 +37,11 @@ void Goalie::goalie(int plusang) { ball->b = ball->dir; storcimentoPorta(); - if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc); - else { + //if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc); + //else { drive->prepareDrive(ball->dir, 350, 0); - cstorc = 0; - } + // cstorc = 0; + //} } } diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index 922b197..6cdc079 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -373,7 +373,15 @@ void PositionSysZone::testLogicZone(){ void PositionSysZone::goCenter() { - if (zoneIndex == 8) + 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 (zoneIndex == 8) drive->prepareDrive(330, GOCENTER_VEL); if (zoneIndex == 7) drive->prepareDrive(0, GOCENTER_VEL); @@ -390,7 +398,7 @@ void PositionSysZone::goCenter() { if (zoneIndex == 1) drive->prepareDrive(180, GOCENTER_VEL); if (zoneIndex == 0) - drive->prepareDrive(135, GOCENTER_VEL); + drive->prepareDrive(135, GOCENTER_VEL); */ } diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 7984d9e..4a16bd5 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -40,7 +40,7 @@ blue_led.on() thresholds = [ (30, 70, -12, 19, 10, 57), # thresholds yellow goal - (0, 31, -2, 39, -68, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (0, 44, -5, 42, -65, -13)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -62,7 +62,7 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+3) sensor.set_saturation(0) -sensor.set_brightness(0) +sensor.set_brightness(-2) sensor.set_quality(0) sensor.set_auto_exposure(False, 10000) sensor.set_auto_gain(True) diff --git a/utility/OpenMV/main_test_conic.py b/utility/OpenMV/main_test_conic.py index 5505073..f2bfe21 100644 --- a/utility/OpenMV/main_test_conic.py +++ b/utility/OpenMV/main_test_conic.py @@ -29,13 +29,8 @@ blue_led.on() #thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal # (30, 45, 1, 40, -60, -19)] # thresholds blue goal # -<<<<<<< HEAD -thresholds = [ (49, 84, -8, 31, 20, 80), # thresholds yellow goal - (0, 51, -4, 44, -59, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) -======= 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) ->>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf roi = (0, 6, 318, 152) @@ -57,11 +52,7 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+2) sensor.set_saturation(+1) -<<<<<<< HEAD sensor.set_brightness(-3) -======= -sensor.set_brightness(-2) ->>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf sensor.set_quality(0) sensor.set_auto_exposure(False, 6000) sensor.set_auto_gain(True) @@ -84,11 +75,11 @@ while(True): blue_led.off() - tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata + 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=150, area_threshold=150, merge = True): + 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()) diff --git a/utility/OpenMV/main_test_conic.py.autosave b/utility/OpenMV/main_test_conic.py.autosave deleted file mode 100644 index f2bfe21..0000000 --- a/utility/OpenMV/main_test_conic.py.autosave +++ /dev/null @@ -1,117 +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) - - -# 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())