EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

camera: correct a critical bug in goal seen logic

robocup
EmaMaker 2021-06-25 20:46:03 +02:00
parent f926ecf027
commit b3dbc4e213
6 changed files with 42 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/behaviour_control/status_vector.h
View File

@ -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;

2
include/sensors/data_source_camera_conicmirror.h
View File

@ -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{

4
include/strategy_roles/games.h
View File

@ -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;
g_extr Game* st_tc3;
g_extr Game* tc3_2;

12
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -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(){

4
src/system/positions/positionsys_camera.cpp
View File

@ -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;
}
}

57
utility/OpenMV/conic_eff_h7.py
View File

@ -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)