From 00f619225594b747fa2f648abc143b2acfd5a45a Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Thu, 27 Feb 2020 17:30:10 +0100 Subject: [PATCH] now the robot can use the camera to recenter even when one the goals can't be seen --- include/positionsys_camera.h | 6 ++-- src/data_source_camera_conicmirror.cpp | 24 +++++++------- src/main.cpp | 2 -- src/positionsys_camera.cpp | 43 ++++++++++++++++---------- utility/OpenMV/conic_eff.py | 12 +++---- 5 files changed, 48 insertions(+), 39 deletions(-) diff --git a/include/positionsys_camera.h b/include/positionsys_camera.h index cc4251d..9fb8cc6 100644 --- a/include/positionsys_camera.h +++ b/include/positionsys_camera.h @@ -2,9 +2,8 @@ #include "systems.h" #define CAMERA_CENTER_X 0 -#define CAMERA_CENTER_Y_BOTH 3 -#define CAMERA_CENTER_Y_BLUE -60 -#define CAMERA_CENTER_Y_YELLOW -30 +#define CAMERA_CENTER_Y -13 +#define CAMERA_CENTER_Y_ABS_SUM 72 #define Kpx 1 #define Kix 0 @@ -22,6 +21,7 @@ class PositionSysCamera : public PositionSystem{ void test() override; void setCameraPID(); void CameraPID(); + int calcOtherGoalY(int goalY); double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index 6946edc..a08f874 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -52,8 +52,8 @@ void DataSourceCameraConic ::readSensor() { 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) * (true_yy) + (true_xy) * (true_xy)); + bDist = sqrt((true_yb) * (true_yb) + (true_xb) * (true_xb)); //Important: update status vector CURRENT_INPUT_WRITE.cameraByte = value; @@ -68,14 +68,10 @@ void DataSourceCameraConic ::readSensor() { 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 (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; @@ -132,7 +128,9 @@ void DataSourceCameraConic::test() DEBUG.print(" | "); DEBUG.print(bAngleFix); DEBUG.print(" | "); - DEBUG.println(bDist); + DEBUG.print(bDist); + DEBUG.print(" | "); + DEBUG.println(CURRENT_DATA_READ.bSeen); DEBUG.println(" --- "); DEBUG.print("Yellow: "); @@ -140,7 +138,9 @@ void DataSourceCameraConic::test() DEBUG.print(" | "); DEBUG.print(yAngleFix); DEBUG.print(" | "); - DEBUG.println(yDist); + DEBUG.print(yDist); + DEBUG.print(" | "); + DEBUG.println(CURRENT_DATA_READ.ySeen); DEBUG.println("---------------"); DEBUG.print("Data: "); DEBUG.print(true_xb); diff --git a/src/main.cpp b/src/main.cpp index 077a193..50ea14e 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,8 +24,6 @@ void loop() { keeper->play(role==0); // Last thing to do: movement and update status vector - // drive->prepareDrive(0,0, CURRENT_DATA_READ.angleAtkFix); - drive->drivePrepared(); updateStatusVector(); } diff --git a/src/positionsys_camera.cpp b/src/positionsys_camera.cpp index 1a63c3b..17b6fa2 100644 --- a/src/positionsys_camera.cpp +++ b/src/positionsys_camera.cpp @@ -63,36 +63,47 @@ void PositionSysCamera :: setCameraPID(){ Y->SetSampleTime(2); } +/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one +We know the sum of the absolute values is a fixed number. +By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y +The sign of the goal y we found is simply the reverse of the one we got +*/ +int PositionSysCamera::calcOtherGoalY(int goalY){ + int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY); + otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY; + return otherGoalY; +} + void PositionSysCamera :: CameraPID(){ if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2; Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; - Setpointx = CAMERA_CENTER_X; - Setpointy = CAMERA_CENTER_Y_BOTH; - } - if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){ + }else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){ Inputx = CURRENT_DATA_READ.cam_xb; - Inputy = CURRENT_DATA_READ.cam_yb; - Setpointx = CAMERA_CENTER_X; - Setpointy = CAMERA_CENTER_Y_BLUE; - } - if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){ + Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb); + }else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){ Inputx = CURRENT_DATA_READ.cam_xy; - Inputy = CURRENT_DATA_READ.cam_yy; - Setpointx = CAMERA_CENTER_X; - Setpointy = CAMERA_CENTER_Y_YELLOW; + Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy); //Setpointy todo }else{ } + Setpointx = CAMERA_CENTER_X; + Setpointy = CAMERA_CENTER_Y; //TODO: no goal seen X->Compute(); Y->Compute(); - // DEBUG.println(Outputx); + // DEBUG.print(CURRENT_DATA_READ.cam_yb); + // DEBUG.print(" "); + // DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb)); - int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14); - dir = (dir+360) % 360; - drive->prepareDrive(dir, 100, 0); + if(abs(Outputx) <= 1 && abs(Outputy) <= 1){ + drive->prepareDrive(0,0,0); + }else{ + int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14); + dir = (dir+360) % 360; + 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 5c54f24..f8e9663 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (23, 38, -34, -8, -12, 22), # thresholds yellow goal - (69, 98, -14, 30, 66, 113)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +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) roi = (0, 6, 318, 152) @@ -59,12 +59,12 @@ clock = time.clock()''' sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) -sensor.set_contrast(+3) -sensor.set_saturation(2) +sensor.set_contrast(1) +sensor.set_saturation(1) sensor.set_brightness(0) sensor.set_quality(0) sensor.set_auto_whitebal(False) -sensor.set_auto_exposure(False, 5000) +sensor.set_auto_exposure(False, 4500) 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=50, area_threshold=80, merge = True): + 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())