From e0c7c569f6e72ad75c96e608d78be6315e3976a7 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Wed, 26 Feb 2020 16:00:26 +0100 Subject: [PATCH] aaaaaaaaaaaaaaaaA --- include/positionsys_camera.h | 4 +- include/status_vector.h | 3 +- src/data_source_camera_conicmirror.cpp | 194 +++++++++++++------------ src/goalie.cpp | 5 +- src/positionsys_camera.cpp | 22 ++- utility/OpenMV/conic_eff.py | 11 +- utility/OpenMV/main_test_conic.py | 2 +- 7 files changed, 125 insertions(+), 116 deletions(-) diff --git a/include/positionsys_camera.h b/include/positionsys_camera.h index 535d925..59dce44 100644 --- a/include/positionsys_camera.h +++ b/include/positionsys_camera.h @@ -1,7 +1,7 @@ #include "systems.h" -#define CAMERA_CENTER_X 0 -#define CAMERA_CENTER_Y 0 +#define CAMERA_CENTER_X 3 +#define CAMERA_CENTER_Y 6 class PositionSysCamera : public PositionSystem{ diff --git a/include/status_vector.h b/include/status_vector.h index 54b47c6..b8553a5 100644 --- a/include/status_vector.h +++ b/include/status_vector.h @@ -39,13 +39,14 @@ typedef struct data{ yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix, + cam_xb, cam_yb, cam_xy, cam_yy, 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; + byte lineSeen, lineActive; bool mate, ATKgoal, DEFgoal, atkSeen, defSeen, bSeen, ySeen, diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index a710a76..6946edc 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -1,7 +1,8 @@ #include "data_source_camera_conicmirror.h" #include "status_vector.h" -DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){ +DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) +{ true_xb = 0; true_yb = 0; true_xy = 0; @@ -21,88 +22,98 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : D bDist = 0; } -void DataSourceCameraConic :: readSensor(){ - while(ser->available() > 0){ +void DataSourceCameraConic ::readSensor() { + while (ser->available() > 0) { value = (int)ser->read(); //Serial.println(value); - if(value==startp){ - start=true; - count=0; - } - else if(value==endp){ - data_received=false; - if(count=4 && start==true) { - data_received=true; + if (value == startp) { + start = true; + count = 0; + } else if (value == endp) { + data_received = false; + if (count = 4 && start == true) { + data_received = true; - true_xb = xb-50; - true_yb = 50-yb; - true_xy = xy-50; - true_yy = 50-yy; + true_xb = xb - 50; + true_yb = 50 - yb; + true_xy = xy - 50; + true_yy = 50 - yy; //Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them - yAngle = -90-(atan2(true_yy, true_xy) * 180 /3.14); - bAngle = -90-(atan2(true_yb, true_xb)* 180 /3.14); + yAngle = -90 - (atan2(true_yy, true_xy) * 180 / 3.14); + bAngle = -90 - (atan2(true_yb, true_xb) * 180 / 3.14); //Now cast angles to [0, 359] domain angle flip them - yAngle = (yAngle+360)%360; - bAngle = (bAngle+360)%360; + yAngle = (yAngle + 360) % 360; + bAngle = (bAngle + 360) % 360; int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; //Fixes with IMU - yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ; - bAngleFix = ((int) ((bAngle + angleFix*0.8)) + 360) % 360; + yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360; + bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360; - yDist = sqrt( (true_yy-50)*(true_yy-50) + (50-true_xy)*(50-true_xy) ); - bDist = sqrt( (true_yb-50)*(true_yb-50) + (50-true_xb)*(50-true_xb) ); + yDist = sqrt((true_yy - 50) * (true_yy - 50) + (50 - true_xy) * (50 - true_xy)); + bDist = sqrt((true_yb - 50) * (true_yb - 50) + (50 - true_xb) * (50 - true_xb)); + + //Important: update status vector + CURRENT_INPUT_WRITE.cameraByte = value; + CURRENT_DATA_WRITE.cam_xb = true_xb; + CURRENT_DATA_WRITE.cam_yb = true_yb; + CURRENT_DATA_WRITE.cam_xy = true_xy; + CURRENT_DATA_WRITE.cam_yy = true_yy; + CURRENT_DATA_WRITE.yAngle = yAngle; + CURRENT_DATA_WRITE.bAngle = bAngle; + CURRENT_DATA_WRITE.yAngleFix = yAngleFix; + 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; + } } - end=true; - start=false; - }else{ - if(start==true){ - if (count==0) xb=value; - else if (count==1) yb=value; - else if (count==2) xy=value; - else if (count==3) yy=value; + end = true; + start = false; + } + else + { + if (start == true) + { + if (count == 0) + xb = value; + else if (count == 1) + yb = value; + else if (count == 2) + xy = value; + else if (count == 3) + yy = value; count++; } - } - //Important: update status vector - CURRENT_INPUT_WRITE.cameraByte = value; - CURRENT_DATA_WRITE.xb = true_xb; - CURRENT_DATA_WRITE.yb = true_yb; - CURRENT_DATA_WRITE.xy = true_xy; - CURRENT_DATA_WRITE.yy = true_yy; - CURRENT_DATA_WRITE.yAngle = yAngle; - CURRENT_DATA_WRITE.bAngle = bAngle; - CURRENT_DATA_WRITE.yAngleFix = yAngleFix; - 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 DataSource -void DataSourceCameraConic::test(){ - goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu +void DataSourceCameraConic::test() +{ + goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu update(); - DEBUG.print("Blue: "); - DEBUG.print(bAngle); - DEBUG.print(" | "); - DEBUG.print(bAngleFix); - DEBUG.print(" | "); - DEBUG.println(bDist); - DEBUG.println(" --- "); + DEBUG.print("Blue: "); + DEBUG.print(bAngle); + DEBUG.print(" | "); + DEBUG.print(bAngleFix); + DEBUG.print(" | "); + DEBUG.println(bDist); + DEBUG.println(" --- "); - DEBUG.print("Yellow: "); - DEBUG.print(yAngle); - DEBUG.print(" | "); - DEBUG.print(yAngleFix); - DEBUG.print(" | "); - DEBUG.println(yDist); - DEBUG.println("---------------"); - DEBUG.print("Data: "); - DEBUG.print(true_xb); - DEBUG.print("|"); - DEBUG.print(true_yb); - DEBUG.print("|"); - DEBUG.print(true_xy); - DEBUG.print("|"); - DEBUG.println(true_yy); - DEBUG.println("---------------"); - delay(150); + DEBUG.print("Yellow: "); + DEBUG.print(yAngle); + DEBUG.print(" | "); + DEBUG.print(yAngleFix); + DEBUG.print(" | "); + DEBUG.println(yDist); + DEBUG.println("---------------"); + DEBUG.print("Data: "); + DEBUG.print(true_xb); + DEBUG.print("|"); + DEBUG.print(true_yb); + DEBUG.print("|"); + DEBUG.print(true_xy); + DEBUG.print("|"); + DEBUG.println(true_yy); + DEBUG.println("---------------"); + delay(150); } diff --git a/src/goalie.cpp b/src/goalie.cpp index e3fab46..3a74971 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -20,8 +20,9 @@ void Goalie::init(){ } void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(50); - else ((PositionSysCamera*)ps)->goCenter(); + ((PositionSysCamera*)ps)->goCenter(); + // if(ball->ballSeen) this->goalie(50); + // else ((PositionSysCamera*)ps)->goCenter(); } int dir, degrees2; diff --git a/src/positionsys_camera.cpp b/src/positionsys_camera.cpp index 3b64f70..765e0c7 100644 --- a/src/positionsys_camera.cpp +++ b/src/positionsys_camera.cpp @@ -14,11 +14,11 @@ 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); + 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(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 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); @@ -27,17 +27,13 @@ void PositionSysCamera::goCenter(){ // 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; + // 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; - // DEBUG.print(x); - // DEBUG.print(":"); - // DEBUG.println(y); - - // int dir = 90-(atan2(y,x)*180/3.14); + // int dir = -90-(atan2(y*1.5,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/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index bb99381..1bd4edf 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,9 +38,8 @@ blue_led.on() ############################################################################## - -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) +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) roi = (0, 6, 318, 152) @@ -62,9 +61,9 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+3) sensor.set_saturation(0) -sensor.set_brightness(-1) +sensor.set_brightness(-2) sensor.set_quality(0) -sensor.set_auto_exposure(False, 3000) +sensor.set_auto_exposure(False, 6000) sensor.set_auto_gain(True) sensor.skip_frames(time = 300) @@ -84,7 +83,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=30, area_threshold=70, merge = True): + 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()) diff --git a/utility/OpenMV/main_test_conic.py b/utility/OpenMV/main_test_conic.py index e4d6ed0..cbfdea6 100644 --- a/utility/OpenMV/main_test_conic.py +++ b/utility/OpenMV/main_test_conic.py @@ -30,7 +30,7 @@ blue_led.on() # (30, 45, 1, 40, -60, -19)] # thresholds blue goal # 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) + (24, 46, -21, 6, -40, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152)