From 42d73d7e85f0246fd7220cb1d717a8ba05ea9059 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 21 Feb 2020 13:37:32 +0100 Subject: [PATCH] New conic shaped mirror, works better. It's higher and has more noticeable changes on the y axis --- include/data_source_camera_conicmirror.h | 2 +- include/drivecontroller.h | 13 +- include/goalie.h | 3 + include/positionsys_camera.h | 14 ++ include/positionsys_zone.h | 3 - include/sensors.h | 2 +- include/status_vector.h | 14 +- include/vars.h | 2 + src/data_source_camera_conicmirror.cpp | 14 +- src/drivecontroller.cpp | 9 +- src/games.cpp | 5 +- src/goalie.cpp | 18 +-- src/main.cpp | 2 +- src/positionsys_camera.cpp | 43 ++++++ src/positionsys_zone.cpp | 10 +- src/sensors.cpp | 4 +- utility/OpenMV/conic_eff.py | 10 +- utility/OpenMV/dist_test.py | 161 +++++++++++++++++++++++ utility/OpenMV/main_test_conic.py | 6 +- 19 files changed, 289 insertions(+), 46 deletions(-) create mode 100644 include/positionsys_camera.h create mode 100644 src/positionsys_camera.cpp create mode 100644 utility/OpenMV/dist_test.py diff --git a/include/data_source_camera_conicmirror.h b/include/data_source_camera_conicmirror.h index aa32665..8dc8d23 100755 --- a/include/data_source_camera_conicmirror.h +++ b/include/data_source_camera_conicmirror.h @@ -4,10 +4,10 @@ #define startp 105 #define endp 115 +#define unkn 116 //Coords are mapped from 0 up to this value #define MAP_MAX 100 #define HALF_MAP_MAX 50 -//#define unkn 0b01101001 class DataSourceCameraConic : public DataSource{ diff --git a/include/drivecontroller.h b/include/drivecontroller.h index 518accc..0484847 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -6,9 +6,17 @@ //PID Constants #define KP 1.5 -#define KI 0 +#define KI 0.2 #define KD 0.1 +#define KSPD 0.3 + +//BEST NUMBERS YET +//USE MOVING AVERAGE AND ANGLE WRAP +// #define KP 1.5 +// #define KI 0 +// #define KD 0.1 + #define UNLOCK_THRESH 800 class DriveController{ @@ -33,8 +41,7 @@ class DriveController{ Motor* m3; Motor* m4; PID* pid; - int pDir, pSpeed, pTilt; - int gDir, gSpeed, gTilt; + int pDir, pSpeed, pTilt, oldSpeed; float x, y, vx, vy, speed1, speed2, speed3, speed4, pidfactor, delta; double input, output, setpoint; diff --git a/include/goalie.h b/include/goalie.h index 0524463..d92021e 100755 --- a/include/goalie.h +++ b/include/goalie.h @@ -4,6 +4,9 @@ #include "sensors.h" #include "data_source_camera_vshapedmirror.h" +#define TILT_MULT 1.8 +#define TILT_DIST 180 +#define CATCH_DIST 150 #define GOALIE_ATKSPD_LAT 255 #define GOALIE_ATKSPD_BAK 350 #define GOALIE_ATKSPD_FRT 345 diff --git a/include/positionsys_camera.h b/include/positionsys_camera.h new file mode 100644 index 0000000..535d925 --- /dev/null +++ b/include/positionsys_camera.h @@ -0,0 +1,14 @@ +#include "systems.h" + +#define CAMERA_CENTER_X 0 +#define CAMERA_CENTER_Y 0 + +class PositionSysCamera : public PositionSystem{ + + public: + PositionSysCamera(); + void goCenter(); + void update() override; + void test() override; + +}; diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h index 71d736f..c7d71d6 100644 --- a/include/positionsys_zone.h +++ b/include/positionsys_zone.h @@ -55,9 +55,6 @@ #define SOUTH_CENTER 8 #define SOUTH_EAST 9 -#define CAMERA_CENTER_X 3 -#define CAMERA_CENTER_Y 6 - class PositionSysZone : public PositionSystem{ public: PositionSysZone(); diff --git a/include/sensors.h b/include/sensors.h index 1782296..d71275e 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl; s_extr DataSourceBNO055* compass; s_extr DataSourceBall* ball; -s_extr DataSourceCameraVShaped* camera; +s_extr DataSourceCameraConic* camera; s_extr DriveController* drive; s_extr DataSourceBT* bt; diff --git a/include/status_vector.h b/include/status_vector.h index c940f54..54b47c6 100644 --- a/include/status_vector.h +++ b/include/status_vector.h @@ -35,13 +35,21 @@ typedef struct input{ }input; typedef struct data{ - int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix, - speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role, axisBlock[4]; + int IMUAngle, ballAngle, ballDistance, + yAngle, bAngle, yAngleFix, bAngleFix, + yDist, bDist, + angleAtk, angleAtkFix, angleDef, angleDefFix, + speed, tilt, dir, axisBlock[4], + USfr, USsx, USdx, USrr, + lineOutDir, matePos, role; Game* game; LineSystem* lineSystem; PositionSystem* posSystem; byte xb, yb, xy, yy, lineSeen, lineActive; - bool mate, ATKgoal, DEFgoal, ballSeen; + bool mate, + ATKgoal, DEFgoal, + atkSeen, defSeen, bSeen, ySeen, + ballSeen; }data; sv_extr input inputs[dim]; diff --git a/include/vars.h b/include/vars.h index 560cb30..4c4e00f 100755 --- a/include/vars.h +++ b/include/vars.h @@ -1,6 +1,8 @@ #pragma once #define DEBUG Serial3 +#define GLOBAL_SPD_MULT 1.0 + #define LED_R 20 #define LED_Y 17 #define LED_G 13 diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index 65fc83a..a710a76 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -78,29 +78,39 @@ void DataSourceCameraConic :: readSensor(){ CURRENT_DATA_WRITE.bAngleFix = bAngleFix; CURRENT_DATA_WRITE.yDist = yDist; CURRENT_DATA_WRITE.bDist = bDist; + + if(xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false; + else CURRENT_DATA_WRITE.bSeen = true; + if(xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false; + else CURRENT_DATA_WRITE.ySeen = true; + if(goalOrientation == HIGH){ CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle; CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix; + CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen; CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle; CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix; + CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen; }else{ CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle; CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix; + CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen; CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle; CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix; + CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen; } } } -// int DataSourceCameraConic::getValueAtk(bool fixed){ +// int DataSource void DataSourceCameraConic::test(){ goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index f9df6c5..71f28ee 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -63,6 +63,9 @@ float DriveController::torad(float f){ void DriveController::drive(int dir, int speed, int tilt){ + speed = (speed * KSPD + oldSpeed * (1-KSPD))*GLOBAL_SPD_MULT; + tilt = tilt > 180 ? tilt - 360 : tilt; + vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); @@ -82,11 +85,11 @@ void DriveController::drive(int dir, int speed, int tilt){ speed4 = -(speed2); // calcola l'errore di posizione rispetto allo 0 - delta = compass->getValue(); + delta = CURRENT_DATA_READ.IMUAngle; if(delta > 180) delta = delta - 360; input = delta; - setpoint = 0; + setpoint = tilt; pid->Compute(); @@ -106,6 +109,8 @@ void DriveController::drive(int dir, int speed, int tilt){ m3->drive((int) speed3); m4->drive((int) speed4); + oldSpeed = speed; + CURRENT_DATA_WRITE.dir = dir; CURRENT_DATA_WRITE.speed = speed; CURRENT_DATA_WRITE.tilt = tilt; diff --git a/src/games.cpp b/src/games.cpp index 93a0253..5de6efc 100644 --- a/src/games.cpp +++ b/src/games.cpp @@ -3,11 +3,12 @@ #include "games.h" #include "linesys_2019.h" #include "positionsys_zone.h" +#include "positionsys_camera.h" void initGames(){ vector lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) }; vector lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) }; - goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysZone()); - keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysZone()); + goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysCamera()); + keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysCamera()); } \ No newline at end of file diff --git a/src/goalie.cpp b/src/goalie.cpp index 7659488..e3fab46 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -3,7 +3,7 @@ #include "vars.h" #include "status_vector.h" #include "math.h" -#include "positionsys_zone.h" +#include "positionsys_camera.h" Goalie::Goalie() : Game() { init(); @@ -20,15 +20,15 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(45); - else ((PositionSysZone*)ps)->goCenter(); + if(ball->ballSeen) this->goalie(50); + else ((PositionSysCamera*)ps)->goCenter(); } int dir, degrees2; void Goalie::goalie(int plusang) { - if(ball->distance < 160) drive->prepareDrive(ball->angle, 350, 0); + if(ball->distance < CATCH_DIST) drive->prepareDrive(ball->angle, 350, 0); else{ - if(ball->angle > 340 || ball->angle < 20) plusang *= 0.15; + if(ball->angle > 345 || ball->angle < 15) plusang *= 0.15; if(ball->angle > 180) degrees2 = ball->angle - 360; else degrees2 = ball->angle; @@ -39,7 +39,7 @@ void Goalie::goalie(int plusang) { else dir = dir; storcimentoPorta(); - if(ball->distance > 185 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc); + if(ball->distance > TILT_DIST && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc); else { drive->prepareDrive(dir, 350, 0); cstorc = 0; @@ -48,8 +48,8 @@ void Goalie::goalie(int plusang) { } void Goalie::storcimentoPorta() { - if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9; - else if (CURRENT_DATA_READ.angleAtkFix <= -10 && CURRENT_DATA_READ.angleAtkFix >= -90) cstorc-=9; - // else cstorc *= 0.7; + if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9; + else if (CURRENT_DATA_READ.angleAtkFix <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9; + else cstorc *= 0.9; cstorc = constrain(cstorc, -45, 45); } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 6cf48f9..ca69d9f 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,7 +24,7 @@ void loop() { keeper->play(role==0); // Last thing to do: movement and update status vector - drive->prepareDrive(0,0,0); + // drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix); drive->drivePrepared(); updateStatusVector(); } diff --git a/src/positionsys_camera.cpp b/src/positionsys_camera.cpp new file mode 100644 index 0000000..3b64f70 --- /dev/null +++ b/src/positionsys_camera.cpp @@ -0,0 +1,43 @@ +#include "positionsys_camera.h" +#include "status_vector.h" +#include "vars.h" +#include "sensors.h" + +PositionSysCamera::PositionSysCamera() {} + +void PositionSysCamera::update(){ +} + +void PositionSysCamera::test(){ +} + +void PositionSysCamera::goCenter(){ + /*WORKS BUT CAN BE BETTER*/ + //Y + if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); + else if ((camera->true_yb + camera->true_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); + //X + else if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); + else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); + else drive->prepareDrive(0, 0, 0); + + + /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/ + // int x = 1; + // int y = 1; + + // //Trying using an angle + // if((CURRENT_DATA_READ.yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy) < -CAMERA_CENTER_Y) y = CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy; + // if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xb; + // if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xy; + + // DEBUG.print(x); + // DEBUG.print(":"); + // DEBUG.println(y); + + // int dir = 90-(atan2(y,x)*180/3.14); + // dir = (dir+360) % 360; + // DEBUG.println(dir); + // drive->prepareDrive(dir, 100, 0); + +} \ No newline at end of file diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index 7cc1f0e..1ec2ebb 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -374,14 +374,6 @@ void PositionSysZone::testLogicZone(){ void PositionSysZone::goCenter() { - // if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); - // else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); - // else drive->prepareDrive(0, 0, 0); -/* if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); - else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); - else drive->prepareDrive(0, 0, 0); */ - /* - PREVIOUS if (zoneIndex == 8) drive->prepareDrive(330, GOCENTER_VEL); if (zoneIndex == 7) @@ -399,7 +391,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/src/sensors.cpp b/src/sensors.cpp index 44f50bd..1e72cdb 100755 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -15,7 +15,7 @@ void initSensors(){ drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); compass = new DataSourceBNO055(); ball = new DataSourceBall(&Serial4, 57600); - camera = new DataSourceCameraVShaped(&Serial2, 19200); + camera = new DataSourceCameraConic(&Serial2, 19200); usCtrl = new DataSourceCtrl(dUs); bt = new DataSourceBT(&Serial3, 115200); } @@ -26,6 +26,6 @@ void updateSensors(){ compass->update(); ball->update(); - // camera->update(); + camera->update(); usCtrl->update(); } \ No newline at end of file diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 675466e..bb99381 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -39,8 +39,8 @@ blue_led.on() -thresholds = [ (26, 74, -11, 6, 17, 50), # thresholds yellow goal - (12, 44, -34, 42, -105, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (40, 100, -14, 21, 16, 69), # thresholds yellow goal + (14, 46, -11, 12, -47, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -62,9 +62,9 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+3) sensor.set_saturation(0) -sensor.set_brightness(-2) +sensor.set_brightness(-1) sensor.set_quality(0) -sensor.set_auto_exposure(False, 10000) +sensor.set_auto_exposure(False, 3000) sensor.set_auto_gain(True) sensor.skip_frames(time = 300) @@ -84,7 +84,7 @@ while(True): 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=75, area_threshold=130, merge = True): + for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=70, merge = True): img.draw_rectangle(blob.rect()) img.draw_cross(blob.cx(), blob.cy()) diff --git a/utility/OpenMV/dist_test.py b/utility/OpenMV/dist_test.py new file mode 100644 index 0000000..2829663 --- /dev/null +++ b/utility/OpenMV/dist_test.py @@ -0,0 +1,161 @@ +# goal dist tracking with conic mirror - By: EmaMaker - fri 21 feb 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) + +def torad(f): + return (f*math.pi/180) % math.pi + +#These measures are in centimeters +FIELD_W = 131 +FIELD_H = 193 +GOALS_DEPTH = 207 + +#Attack 1 means attacking yellow, attack 0 means attacking blue +ATTACKING = 0 + +# 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 = [ (40, 100, -3, 35, 16, 96) , # thresholds yellow goal + (39, 59, -13, 12, -43, -19)] # 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(2) +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=45, 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() ) ] + 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] + cx = img.width() / 2 - cx + cy = img.height() / 2 - cy + yAngle = math.pi/2 - math.atan2(cy, cx) + yDist = math.sqrt(cx*cx + cy*cy) + string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(yAngle)+" | "+str(yDist)+str(area)+"y" + #print (string_yellow) # test on serial terminal + + '''Blue''' + area,cx,cy,code = tt_blue[nb-1] + cx = img.width() / 2 - cx + cy = img.height() / 2 - cy + bAngle = math.pi/2 - math.atan2(cy, cx) + bDist = math.sqrt(cx*cx + cy*cy) + string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(bAngle)+" | "+str(bDist)+str(area)+"b" + #print (string_blue) # test on serial terminal + + #Now calculate distance and position + #Goal 1 is the one in front of the robot + #Goal 2 is the one facing the back of the robot + + #convert in [0, 360), to be sure + bAngle = int(bAngle * 180 / math.pi) + yAngle = int(yAngle * 180 / math.pi) + + bAngle = (bAngle + 360) % 360; + yAngle = (yAngle + 360) % 360; + + #Now bring it back to [-179, 180] + if bAngle > 180: + bAngle = bAngle - 360 + + if yAngle > 180: + yAngle = yAngle - 360 + + if ATTACKING == 1: + angle1 = abs(yAngle) + angle2 = abs(bAngle - 180) + else: + angle1 = abs(bAngle) + angle2 = abs(yAngle - 180) + + dist1 = (GOALS_DEPTH * math.sin(angle2) ) / (math.sin(angle1+angle2)) + dist2 = (GOALS_DEPTH * math.sin(angle1) ) / (math.sin(angle1+angle2)) + + print("------") + print(angle1) + print(angle2) + print(dist1) + print(dist2) + print("------") + + + #print ("..................................") + +print(clock.fps()) diff --git a/utility/OpenMV/main_test_conic.py b/utility/OpenMV/main_test_conic.py index f2bfe21..e4d6ed0 100644 --- a/utility/OpenMV/main_test_conic.py +++ b/utility/OpenMV/main_test_conic.py @@ -29,8 +29,8 @@ blue_led.on() #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) +thresholds = [ (57, 93, -18, 14, 28, 77) , # thresholds yellow goal + (31, 68, -20, 18, -47, -17)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) @@ -79,7 +79,7 @@ while(True): 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): + for blob in img.find_blobs(thresholds, pixels_threshold=70, area_threshold=100, merge = True): img.draw_rectangle(blob.rect()) img.draw_cross(blob.cx(), blob.cy())