From b3dbc4e213469da73afa52209802c0d53161ebca Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 25 Jun 2021 20:46:03 +0200 Subject: [PATCH] camera: correct a critical bug in goal seen logic --- include/behaviour_control/status_vector.h | 8 +-- .../sensors/data_source_camera_conicmirror.h | 2 +- include/strategy_roles/games.h | 4 +- .../data_source_camera_conicmirror.cpp | 12 ++-- src/system/positions/positionsys_camera.cpp | 4 +- utility/OpenMV/conic_eff_h7.py | 57 +++++++++---------- 6 files changed, 42 insertions(+), 45 deletions(-) diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 5c70039..3d4ebee 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -40,11 +40,11 @@ typedef struct data{ yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix, - cam_xb, cam_yb, cam_xy, cam_yy, + cam_xb = 0, cam_yb = 0, cam_xy = 0, cam_yy = 0, speed, tilt, dir, axisBlock[4], USfr, USsx, USdx, USrr, - lineOutDir, matePos, role, cam_xb_fixed, - cam_xy_fixed, cam_yb_fixed, cam_yy_fixed, + lineOutDir, matePos, role, cam_xb_fixed = 0, + cam_xy_fixed = 0, cam_yb_fixed = 0, cam_yy_fixed = 0, posx, posy, addvx, addvy; Game* game; LineSystem* lineSystem; @@ -52,7 +52,7 @@ typedef struct data{ byte lineSeen, lineActive; bool mate, ATKgoal, DEFgoal, - atkSeen, defSeen, bSeen, ySeen, + atkSeen, defSeen, bSeen = true, ySeen = false, ballSeen, ballPresent; }data; diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 8fbff0d..1505b04 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -28,7 +28,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/ //Robot with roller #define CAMERA_TRANSLATION_X 0 -#define CAMERA_TRANSLATION_Y -3 +#define CAMERA_TRANSLATION_Y 0 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 2020856..652ac29 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -15,6 +15,7 @@ #include "strategy_roles/spot_finder.h" #include "strategy_roles/the_spinner.h" #include "strategy_roles/round_robin.h" +#include "strategy_roles/corner_kick_2.h" // #include "strategy_roles/keeper.h" void initGames(); @@ -28,4 +29,5 @@ g_extr Game* pass_and_shoot; g_extr Game* tc1; g_extr Game* tc2; g_extr Game* st_tc1; -g_extr Game* st_tc3; \ No newline at end of file +g_extr Game* st_tc3; +g_extr Game* tc3_2; \ No newline at end of file diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index e64698e..376fe4a 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -85,6 +85,9 @@ void DataSourceCameraConic ::readSensor() { void DataSourceCameraConic ::computeCoordsAngles() { + CURRENT_DATA_WRITE.bSeen = true_xb != unkn && true_yb != unkn; + CURRENT_DATA_WRITE.ySeen = true_xy != unkn && true_yy != unkn; + //Where are the goals relative to the robot? //Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them, getting the original coords calculated by the camera true_xb = 50 - true_xb + CAMERA_TRANSLATION_X*0.5; @@ -150,12 +153,7 @@ void DataSourceCameraConic ::computeCoordsAngles() { 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; @@ -201,7 +199,7 @@ void DataSourceCameraConic ::computeCoordsAngles() { byte to_32u4 = 0; to_32u4 |= (CURRENT_DATA_READ.ySeen); to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1; - // BALL_32U4.write(to_32u4); + BALL_32U4.write(to_32u4); } void DataSourceCameraConic::test(){ diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 6a8358f..3f910f2 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -54,8 +54,7 @@ void PositionSysCamera::update(){ posx *= -1; posy *= -1; - //Filtering error in calculation like this is a dirty hack, I know - if(posx < -MAX_X || posx > MAX_X || posy < -MAX_Y || posy > MAX_Y || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) { + if(CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false ) { // Go back in time until we found a valid status, when we saw at least one goal int i = 1; do{ @@ -70,7 +69,6 @@ void PositionSysCamera::update(){ // Trick the status vector into thinking this was a valid status CURRENT_DATA_WRITE.ySeen = valid_data.ySeen; CURRENT_DATA_WRITE.bSeen = valid_data.bSeen; - } } diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index 1cdf301..33f3d23 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -83,7 +83,7 @@ clock = time.clock() while(True): clock.tick() - print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db())) + #print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db())) blue_led.off() @@ -174,40 +174,39 @@ while(True): #Prepare for send as a list of characters s_bcx = chr(b_cx) s_bcy = chr(b_cy) - - '''index = 1 - if b_found == True: - while nb-index >= 0: - b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index] - - index += 1 - # If the two blobs are on opposide side of the field, everything is good - if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))): - - img.draw_cross(b1_cx, b1_cy) - - b_cx = int(b1_cy - img.height() / 2) - b_cy = int(img.width() / 2 - b1_cx) - - print("before :" + str(b_cx) + " " + str(b_cy)) - b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0) - b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100) - - print("after :" + str(b_cx) + " " + str(b_cy)) - - #Prepare for send as a list of characters - s_bcx = chr(b_cx) - s_bcy = chr(b_cy) - - break else: b_cx = BYTE_UNKNOWN b_cy = BYTE_UNKNOWN #Prepare for send as a list of characters s_bcx = b_cx - s_bcy = b_cy''' + s_bcy = b_cy + '''index = 1 + if b_found == True: + while nb-index >= 0: + b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index] - #print(str(y_cx) + " | " + str(y_cy) + " --- " + str(b_cx) + " | " + str(b_cy)) + index += 1 + # If the two blobs are on opposide side of the field, everything is good + if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))): + + img.draw_cross(b1_cx, b1_cy) + + b_cx = int(b1_cy - img.height() / 2) + b_cy = int(img.width() / 2 - b1_cx) + + print("before :" + str(b_cx) + " " + str(b_cy)) + b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0) + b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100) + + print("after :" + str(b_cx) + " " + str(b_cy)) + + #Prepare for send as a list of characters + s_bcx = chr(b_cx) + s_bcy = chr(b_cy) + + break''' + + print(str(s_ycx) + " | " + str(s_ycy) + " --- " + str(s_bcx) + " | " + str(s_bcy)) uart.write(START_BYTE) uart.write(s_bcx)