From 14992f5baa60d81f80b439d07f401a0abb21cd6a Mon Sep 17 00:00:00 2001 From: emamaker Date: Mon, 4 Jul 2022 15:31:01 +0200 Subject: [PATCH 01/14] striker: adapt striker to new ball reading --- include/sensors/data_source_ball.h | 4 ++-- include/strategy_roles/striker.h | 34 +++++++++++++++--------------- src/strategy_roles/striker.cpp | 24 ++++++++++++--------- 3 files changed, 33 insertions(+), 29 deletions(-) diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index 7e84456..df478ac 100644 --- a/include/sensors/data_source_ball.h +++ b/include/sensors/data_source_ball.h @@ -2,8 +2,8 @@ #include "behaviour_control/data_source.h" #include -#define MOUTH_MIN_ANGLE 345 -#define MOUTH_MAX_ANGLE 15 +#define MOUTH_MIN_ANGLE 350 +#define MOUTH_MAX_ANGLE 10 #define MOUTH_DISTANCE 100 #define MOUTH_MAX_DISTANCE 140 diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index fc91130..2ed68d0 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -1,32 +1,32 @@ #pragma once -#include "sensors/data_source_camera_vshapedmirror.h" +#include "behaviour_control/complementary_filter.h" +#include "motors_movement/drivecontroller.h" #include "sensors/sensors.h" #include "strategy_roles/game.h" -#include "motors_movement/drivecontroller.h" #define STRIKER_ATTACK_DISTANCE 110 #define STRIKER_TILT_STOP_DISTANCE 140 -#define STRIKER_PLUSANG 50 +#define STRIKER_PLUSANG 35 #define STRIKER_PLUSANG_VISIONCONE 7 #define STRIKER_VEL MAX_VEL -#define STRIKER_MOUTH_SX 345 -#define STRIKER_MOUTH_DX 15 +class Striker : public Game +{ -class Striker : public Game{ +public: + Striker(); + Striker(LineSystem *ls, PositionSystem *ps); - public: - Striker(); - Striker(LineSystem* ls, PositionSystem* ps); +private: + void realPlay() override; + void init() override; + void striker(); + int tilt(); + float ballTilt(); - private: - void realPlay() override; - void init() override; - void striker(); - int tilt(); - float ballTilt(); + int atk_speed, atk_direction = 0, atk_tilt, stato = 0, plusang_flag = 0; + bool flag = false; - int atk_speed, atk_direction = 0, atk_tilt, stato = 0, plusang_flag = 0; - bool flag = false; + ComplementaryFilter *ball_filter; }; diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 6b982cb..81c671b 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -18,9 +18,7 @@ Striker::Striker(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) void Striker::init() { - atk_speed = 0; - atk_direction = 0; - atk_tilt = 0; + ball_filter = new ComplementaryFilter(0.75f); } void Striker::realPlay() @@ -39,11 +37,11 @@ unsigned long ttilt = 0; void Striker::striker() { - if(CURRENT_DATA_READ.lineActive != 0 ) flag = false; + if (CURRENT_DATA_READ.lineActive != 0) flag = false; - int dir = 0, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG + 8 * (CURRENT_DATA_READ.ballDistance <= 90); + int dir = 0, ball_deg = ball_filter->calculate(CURRENT_DATA_READ.ballAngle), plusang = STRIKER_PLUSANG + 10 * (CURRENT_DATA_READ.ballDistance <= 90); - if (CURRENT_DATA_READ.ballDistance >= 125) + if (CURRENT_DATA_READ.ballDistance >= 120) { drive->prepareDrive(ball_deg > 180 ? ball_deg * 0.96 : ball_deg * 1.04, STRIKER_VEL, 0); } @@ -53,6 +51,8 @@ void Striker::striker() if (ball->isInFront()) { + plusang = STRIKER_PLUSANG_VISIONCONE; + // dir = ball_deg >= 0 ? plusang : - plusang; dir = 0; flag = false; } @@ -60,7 +60,7 @@ void Striker::striker() { if (!flag) { - + if (ball_deg <= 90) dir = ball_deg + plusang; else if (ball_deg >= 270) @@ -73,11 +73,14 @@ void Striker::striker() else plusang_flag = -plusang; } - }else dir = ball_deg + plusang_flag; + } + else + dir = ball_deg + plusang_flag; } + if (CURRENT_DATA_READ.atkSeen) atk_tilt = CURRENT_DATA_READ.angleAtkFix; dir = (dir + 360) % 360; - drive->prepareDrive(dir, STRIKER_VEL, tilt()); + drive->prepareDrive(dir, STRIKER_VEL, atk_tilt); // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); // else roller->speed(roller->MIN); @@ -86,5 +89,6 @@ void Striker::striker() int Striker::tilt() { - return CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0; + // return CURRENT_DATA_READ.ballAngle <= 110 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0; + return CURRENT_DATA_READ.angleAtkFix; } \ No newline at end of file From c2c16515c3c77ce5779c779da1c7d4aa81fdd19e Mon Sep 17 00:00:00 2001 From: emamaker Date: Tue, 5 Jul 2022 16:28:26 +0200 Subject: [PATCH 02/14] ball: mouth is smaller --- include/sensors/data_source_ball.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index df478ac..7e84456 100644 --- a/include/sensors/data_source_ball.h +++ b/include/sensors/data_source_ball.h @@ -2,8 +2,8 @@ #include "behaviour_control/data_source.h" #include -#define MOUTH_MIN_ANGLE 350 -#define MOUTH_MAX_ANGLE 10 +#define MOUTH_MIN_ANGLE 345 +#define MOUTH_MAX_ANGLE 15 #define MOUTH_DISTANCE 100 #define MOUTH_MAX_DISTANCE 140 From 701789b823188115b018e7bf6fc7ab5e9343c4ee Mon Sep 17 00:00:00 2001 From: emamaker Date: Tue, 5 Jul 2022 16:29:39 +0200 Subject: [PATCH 03/14] camera: no translation needed --- include/sensors/data_source_camera_conicmirror.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 5de0222..3b64f42 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -23,7 +23,7 @@ To overcome this, each coordinate needs to be shifted by some amount, defined on These values need to be subtracted from the coords used in setMoveSetpoints*/ #define CAMERA_TRANSLATION_X 0 -#define CAMERA_TRANSLATION_Y -12 +#define CAMERA_TRANSLATION_Y 0 class DataSourceCameraConic : public DataSource{ From 5deb6a9af18b6880f85ca299b86df984c005be1f Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sun, 3 Jul 2022 12:52:04 +0200 Subject: [PATCH 04/14] camera: switch to using angles and dist --- include/behaviour_control/status_vector.h | 20 +- .../sensors/data_source_camera_conicmirror.h | 43 +-- include/systems/position/positionsys_camera.h | 34 +- .../data_source_camera_conicmirror.cpp | 312 +++++------------- src/strategy_roles/striker.cpp | 12 +- src/system/lines/linesys_camera.cpp | 18 +- src/system/positions/positionsys_camera.cpp | 94 ++++-- src/system/positions/positionsys_zone.cpp | 4 +- src/test_menu.cpp | 9 +- utility/OpenMV/conic_angle_dist.py | 153 +++++++++ 10 files changed, 370 insertions(+), 329 deletions(-) create mode 100644 utility/OpenMV/conic_angle_dist.py diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 9b3540f..5375fac 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -30,30 +30,26 @@ typedef struct input{ int IMUAngle, USfr, USsx, USdx, USrr, BT; - byte ballByte, cameraByte, lineByte, xb, yb, xy, yy; + byte ballByte, lineByte, xb, yb, xy, yy; + char cameraChar; bool SW_DX, SW_SX; }input; typedef struct data{ int IMUAngle = 0, IMUOffset = 0, ballAngle = 0, ballAngleFix, ballDistance, ballPresenceVal, - yAngle, bAngle, yAngleFix, bAngleFix, - yDist, bDist, - angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix, - 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 = 0, - cam_xy_fixed = 0, cam_yb_fixed = 0, cam_yy_fixed = 0, - posx, posy; + lineOutDir, matePos, role; + int yangle, bangle, ydist, bdist, yangle_fix, bangle_fix, atkGAngle, defGAngle, atkGAngle_fix, defGAngle_fix, atkGDist, defGDist; + int posx, posy; float addvx, addvy; Game* game; LineSystem* lineSystem; PositionSystem* posSystem; byte lineSeen, lineActive; - bool mate, - ATKgoal, DEFgoal, - atkSeen, defSeen, bSeen = true, ySeen = false, - ballSeen, ballPresent; + bool mate, + atkSeen = false, defSeen = false, bSeen = false, ySeen = false, ballSeen, ballPresent; + bool camera_back_in_time = false; //whether or not we copied camera position from and older status }data; sv_extr input inputs[dim]; diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 3b64f42..a98a1b8 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -1,29 +1,17 @@ #pragma once +#include #include "behaviour_control/data_source.h" #include "behaviour_control/complementary_filter.h" -#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 -//Imu To Camera Angle Mult -#define IMUTOC_AMULT 1 -#define FILTER_DEFAULT_COEFF 0.25 -#define FILTER_BY_COEFF FILTER_DEFAULT_COEFF -#define FILTER_BX_COEFF FILTER_DEFAULT_COEFF -#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF -#define FILTER_YX_COEFF FILTER_DEFAULT_COEFF - -/*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide -To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time. -These values need to be subtracted from the coords used in setMoveSetpoints*/ - -#define CAMERA_TRANSLATION_X 0 -#define CAMERA_TRANSLATION_Y 0 +#define FILTER_DEFAULT_COEFF 0.6 +#define FILTER_YANGLE_COEFF FILTER_DEFAULT_COEFF +#define FILTER_BANGLE_COEFF FILTER_DEFAULT_COEFF +#define FILTER_YANGLE_FIX_COEFF FILTER_DEFAULT_COEFF +#define FILTER_BANGLE_FIX_COEFF FILTER_DEFAULT_COEFF +#define FILTER_YDIST_COEFF FILTER_DEFAULT_COEFF +#define FILTER_BDIST_COEFF FILTER_DEFAULT_COEFF class DataSourceCameraConic : public DataSource{ @@ -35,15 +23,14 @@ class DataSourceCameraConic : public DataSource{ // int getValueAtk(bool); // int getValueDef(bool); - int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist; - - int count = 0, unkn_counter; - int xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy, true_xb_fixed, - true_xy_fixed, true_yb_fixed, true_yy_fixed; - bool data_received = false, start = false, end = false; + int count = 0, unkn_counter = 0; + bool data_received = false, start = false, end = false, dash = false; + char current_char = 'a', start_char = 'a', end_char = 'a'; //initialize to unused values int goalOrientation, old_goalOrientation, pAtk, pDef; - int value; + String s1 = "", s2 = ""; + + int yangle = 0, bangle = 0, yangle_fix = 0, bangle_fix = 0, ydist = 0, bdist = 0; + ComplementaryFilter *filt_yangle, *filt_bangle, *filt_yangle_fix, *filt_bangle_fix, *filt_ydist, *filt_bdist; - ComplementaryFilter *filter_yy, *filter_xy, *filter_yb, *filter_xb, *filter_yy_fix, *filter_xy_fix, *filter_yb_fix, *filter_xb_fix; }; \ No newline at end of file diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 755c657..b402634 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -7,20 +7,27 @@ //Camera center: those setpoints correspond to what we consider the center of the field #define CAMERA_CENTER_X 0 -#define CAMERA_CENTER_Y -20 - -//Camera goal: those setpoints correspond to the position of the center of the goal on the field -#define CAMERA_GOAL_X 0 -#define CAMERA_GOAL_Y -12 - -#define CAMERA_GOAL_MIN_X -8 +#define CAMERA_CENTER_Y 0 +//left and right limits of a goal #define CAMERA_GOAL_MAX_X 8 +#define CAMERA_GOAL_MIN_X (-8) + +//dimensions of the field, for how we scale it +#define DIM_X 50 +#define DIM_X_HALF 25 +#define DIM_Y 80 +#define DIM_Y_HALF 40 + +//where is the center of a goal blob as seen by openmv on the field. For atk goal it's positive, for def goal it's negative +#define CAMERA_GOAL_X 0 +#define CAMERA_GOAL_Y DIM_Y_HALF +#define CAMERA_GOAL_ATK_Y CAMERA_GOAL_Y +#define CAMERA_GOAL_DEF_Y (-CAMERA_GOAL_Y) + +//hipotenuse of dimensions of field +#define MAX_DIST_EXPERIMENTAL 94 + -#define CAMERA_CENTER_Y_ABS_SUM 60 -//Actually it's ± MAX_VAL -#define MAX_X 50 -#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2) -#define MAX_DIST_EXPERIMENTAL 70 #define DIST_MULT 8 #define VICINITY_DIST_TRESH 2 @@ -45,13 +52,12 @@ class PositionSysCamera : public PositionSystem{ void test() override; void setCameraPID(); void CameraPID(); - int calcOtherGoalY(int goalY); bool isInTheVicinityOf(int, int); bool isInRoughVicinityOf(int, int); bool isAtDistanceFrom(int, int, int); double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; - int MAX_DIST, vx, vy, axisx, axisy; + int MAX_DIST, vx, vy, axisx, axisy, method; bool givenMovement; PID* X; PID* Y; diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index b2d54bc..3475510 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -1,246 +1,112 @@ -#include "behaviour_control/status_vector.h" #include "sensors/data_source_camera_conicmirror.h" +#include "behaviour_control/status_vector.h" #include "vars.h" -//Comment out to disable complementary filters on angles +// Comment out to disable complementary filters on angles #define CAMERA_CONIC_FILTER_POINTS -DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) { - true_xb = 0; - true_yb = 0; - true_xy = 0; - true_yy = 0; - true_xb_fixed = 0; - true_yb_fixed = 0; - true_xy_fixed = 0; - true_yy_fixed = 0; - xb = 0; - yb = 0; - xy = 0; - yy = 0; - start = false; - data_received = false; - end = false; - yAngle = 0; - yAngleFix = 0; - yDist = 0; - bAngle = 0; - bAngleFix = 0; - bDist = 0; - true_xb_fixed = 0; - true_yb_fixed = 0; - true_xy_fixed = 0; - true_yy_fixed = 0; - - goalOrientation = 0; - old_goalOrientation = 0; - - filter_yy = new ComplementaryFilter(FILTER_YY_COEFF); - filter_xy = new ComplementaryFilter(FILTER_YX_COEFF); - filter_yb = new ComplementaryFilter(FILTER_BY_COEFF); - filter_xb = new ComplementaryFilter(FILTER_BX_COEFF); - filter_yy_fix = new ComplementaryFilter(FILTER_YY_COEFF); - filter_xy_fix = new ComplementaryFilter(FILTER_YX_COEFF); - filter_yb_fix = new ComplementaryFilter(FILTER_BY_COEFF); - filter_xb_fix = new ComplementaryFilter(FILTER_BX_COEFF); +DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) +{ + filt_yangle = new ComplementaryFilter(FILTER_YANGLE_COEFF); + filt_bangle = new ComplementaryFilter(FILTER_BANGLE_COEFF); + filt_ydist = new ComplementaryFilter(FILTER_YDIST_COEFF); + filt_bdist = new ComplementaryFilter(FILTER_BDIST_COEFF); + filt_yangle_fix = new ComplementaryFilter(FILTER_YANGLE_FIX_COEFF); + filt_bangle_fix = new ComplementaryFilter(FILTER_BANGLE_FIX_COEFF); } -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; +void DataSourceCameraConic ::readSensor() +{ + while (ser->available() > 0) + { + current_char = ser->read(); - true_xb = xb; - true_yb = yb; - true_xy = xy; - true_yy = yy; + //update status vector + CURRENT_INPUT_WRITE.cameraChar = current_char; - computeCoordsAngles(); - } - 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++; - } + // start + if (current_char == 'B' || current_char == 'Y') + { + start_char = current_char; + start = true; + end = false; + dash = false; + } + // end + else if (current_char == 'b' || current_char == 'y') + { + end_char = current_char; + + if(start && dash && ((start_char == 'B' && end_char == 'b') || (start_char == 'Y' && end_char == 'y') ) ){ + if(start_char == 'B'){ + bangle = s1.toInt(); + bdist = s2.toInt(); + }else{ + yangle = s1.toInt(); + ydist = s2.toInt(); + } + } + + end = true; + start = false; + dash = false; + count = 0; + }// dash + else if (current_char == '-') + { + dash = true; + } // it's a number + else if (isDigit(current_char)) + { + if(dash) s2 += current_char; + else s1 += current_char; + } } - } } +// Angles are received in degrees +void DataSourceCameraConic ::computeCoordsAngles() +{ -void DataSourceCameraConic ::computeCoordsAngles() { - CURRENT_DATA_WRITE.bSeen = true_xb != unkn && true_yb != unkn; - CURRENT_DATA_WRITE.ySeen = true_xy != unkn && true_yy != unkn; + #ifdef CAMERA_CONIC_FILTER_POINTS + yangle = filt_yangle->calculate(yangle); + ydist = filt_ydist->calculate(ydist); + bangle = filt_bangle->calculate(bangle); + bdist = filt_bdist->calculate(bdist); + #endif - //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; - true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y*0.5; - true_xy = 50 - true_xy + CAMERA_TRANSLATION_X*0.5; - true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y*0.5; + // Fix angles using the IMU + int tmp = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; - #ifdef CAMERA_CONIC_FILTER_POINTS - true_xb = filter_xb->calculate(true_xb); - true_yb = filter_yb->calculate(true_yb); - true_xy = filter_xy->calculate(true_xy); - true_yy = filter_yy->calculate(true_yy); - #endif + yangle_fix = (yangle+tmp+360) % 360; + bangle_fix = (bangle+tmp+360) % 360; - //-90 + to account for phase shifting with goniometric circle - 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; - yDist = sqrt((true_yy) * (true_yy) + (true_xy) * (true_xy)); - bDist = sqrt((true_yb) * (true_yb) + (true_xb) * (true_xb)); - // int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; + //TODO: Maybe add a complementary filter on fixed angles ? - // //Fixes with IMU - // yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360; - // bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360; + // Important: update status vector + CURRENT_DATA_WRITE.yangle = yangle; + CURRENT_DATA_WRITE.bangle = bangle; + CURRENT_DATA_WRITE.yangle_fix = yangle_fix; + CURRENT_DATA_WRITE.bangle_fix = bangle_fix; + CURRENT_DATA_WRITE.ydist = ydist; + CURRENT_DATA_WRITE.bdist = bdist; + CURRENT_DATA_WRITE.bSeen = bangle != 999; + CURRENT_DATA_WRITE.ySeen = yangle != 999; - //Rotate the points of the goals on the cartesian plane to compensate the robot tilt - int tmp = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; - //We are considering the angle being negative if going counterclockwise and positive is going clockwise. - //The formula used below assumes the angle being positive in counterclockwise and negative in clockwise, so the angle must be multiplied by -1 - //A little explanation of the formula used here can be found on wikipedia: https://en.wikipedia.org/wiki/Rotation_of_axes - float angleFix = -tmp*3.14/180; - true_xb_fixed = (true_xb*(cos(angleFix))) + (true_yb*(sin(angleFix))); - true_yb_fixed = (-true_xb*(sin(angleFix))) + (true_yb*(cos(angleFix))); - true_xy_fixed = (true_xy*(cos(angleFix))) + (true_yy*(sin(angleFix))); - true_yy_fixed = (-true_xy*(sin(angleFix))) + (true_yy*(cos(angleFix))); + CURRENT_DATA_WRITE.atkGAngle = goalOrientation ? yangle : bangle; + CURRENT_DATA_WRITE.atkGAngle_fix = goalOrientation ? yangle_fix : bangle_fix; + CURRENT_DATA_WRITE.atkGDist = goalOrientation ? ydist : bdist; + CURRENT_DATA_WRITE.atkSeen = goalOrientation ? CURRENT_DATA_WRITE.ySeen : CURRENT_DATA_WRITE.bSeen; - #ifdef CAMERA_CONIC_FILTER_POINTS - true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed); - true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed); - true_xy_fixed = filter_xy_fix->calculate(true_xy_fixed); - true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed); - #endif - - yAngleFix = -90 + (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14); - bAngleFix = -90 + (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14); - - //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.cam_xb_fixed = true_xb_fixed; - CURRENT_DATA_WRITE.cam_yb_fixed = true_yb_fixed; - CURRENT_DATA_WRITE.cam_xy_fixed = true_xy_fixed; - CURRENT_DATA_WRITE.cam_yy_fixed = true_yy_fixed; - 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 (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.distAtk = CURRENT_DATA_WRITE.yDist; - - CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yy; - CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed; - CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xy; - CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xy_fixed; - - CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle; - CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix; - CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen; - CURRENT_DATA_WRITE.distDef = CURRENT_DATA_WRITE.bDist; - - CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yb; - CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yb_fixed; - CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xb; - CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xb_fixed; - } else { - CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle; - CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.bAngleFix; - CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen; - CURRENT_DATA_WRITE.distAtk = CURRENT_DATA_WRITE.bDist; - - CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yb; - CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yb_fixed; - CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xb; - CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xb_fixed; - - CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle; - CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix; - CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen; - CURRENT_DATA_WRITE.distDef = CURRENT_DATA_WRITE.yDist; - - CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yy; - CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yy_fixed; - CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xy; - CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xy_fixed; - } - - // byte to_32u4 = 0; - // to_32u4 |= (CURRENT_DATA_READ.ySeen); - // to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1; - // BALL_32U4.write(to_32u4); + CURRENT_DATA_WRITE.defGAngle = !goalOrientation ? yangle : bangle; + CURRENT_DATA_WRITE.defGAngle_fix = !goalOrientation ? yangle_fix : bangle_fix; + CURRENT_DATA_WRITE.defGDist = !goalOrientation ? ydist : bdist; + CURRENT_DATA_WRITE.defSeen = !goalOrientation ? CURRENT_DATA_WRITE.ySeen : CURRENT_DATA_WRITE.bSeen; } -void DataSourceCameraConic::test(){ - update(); - DEBUG.print("Blue: Angle: "); - DEBUG.print(bAngle); - DEBUG.print(" | Fixed Angle: "); - DEBUG.print(bAngleFix); - DEBUG.print(" | Distance: "); - DEBUG.print(bDist); - DEBUG.print(" | Seen: "); - DEBUG.println(CURRENT_DATA_READ.bSeen); - DEBUG.print(" | Received coords: ("); - DEBUG.print(CURRENT_DATA_READ.cam_xb); - DEBUG.print(","); - DEBUG.print(CURRENT_DATA_READ.cam_yb); - DEBUG.print(")"); - DEBUG.print(" | Fixed coords: ("); - DEBUG.print(CURRENT_DATA_READ.cam_xb_fixed); - DEBUG.print(","); - DEBUG.print(CURRENT_DATA_READ.cam_yb_fixed); - DEBUG.println(")"); - DEBUG.println("----------------"); - DEBUG.print("Yellow: Angle: "); - DEBUG.print(yAngle); - DEBUG.print(" | Fixed Angle: "); - DEBUG.print(yAngleFix); - DEBUG.print(" | Distance: "); - DEBUG.print(yDist); - DEBUG.print(" | Seen: "); - DEBUG.println(CURRENT_DATA_READ.ySeen); - DEBUG.print(" | Received coords: ("); - DEBUG.print(CURRENT_DATA_READ.cam_xy); - DEBUG.print(","); - DEBUG.print(CURRENT_DATA_READ.cam_yy); - DEBUG.print(")"); - DEBUG.print(" | Fixed coords: ("); - DEBUG.print(CURRENT_DATA_READ.cam_xy_fixed); - DEBUG.print(","); - DEBUG.print(CURRENT_DATA_READ.cam_yy_fixed); - DEBUG.println(")"); - DEBUG.println("---------------"); +void DataSourceCameraConic::test() +{ + DEBUG.print("BLUE GOAL:: Seen: " + String(CURRENT_DATA_READ.bSeen) + " | Angle: " + CURRENT_DATA_READ.bangle + " | Fixed Angle: " + CURRENT_DATA_READ.bangle_fix + " | Dist: " + CURRENT_DATA_READ.bdist); + DEBUG.print("YELLOW GOAL:: Seen: " + String(CURRENT_DATA_READ.ySeen) + " | Angle: " + CURRENT_DATA_READ.yangle + " | Fixed Angle: " + CURRENT_DATA_READ.yangle_fix + " | Dist: " + CURRENT_DATA_READ.ydist); delay(150); } diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 81c671b..34805ab 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -39,9 +39,9 @@ void Striker::striker() if (CURRENT_DATA_READ.lineActive != 0) flag = false; - int dir = 0, ball_deg = ball_filter->calculate(CURRENT_DATA_READ.ballAngle), plusang = STRIKER_PLUSANG + 10 * (CURRENT_DATA_READ.ballDistance <= 90); + int dir = 0, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG + 8 * (CURRENT_DATA_READ.ballDistance <= 90); - if (CURRENT_DATA_READ.ballDistance >= 120) + if (CURRENT_DATA_READ.ballDistance >= 125) { drive->prepareDrive(ball_deg > 180 ? ball_deg * 0.96 : ball_deg * 1.04, STRIKER_VEL, 0); } @@ -51,8 +51,6 @@ void Striker::striker() if (ball->isInFront()) { - plusang = STRIKER_PLUSANG_VISIONCONE; - // dir = ball_deg >= 0 ? plusang : - plusang; dir = 0; flag = false; } @@ -78,9 +76,8 @@ void Striker::striker() dir = ball_deg + plusang_flag; } - if (CURRENT_DATA_READ.atkSeen) atk_tilt = CURRENT_DATA_READ.angleAtkFix; dir = (dir + 360) % 360; - drive->prepareDrive(dir, STRIKER_VEL, atk_tilt); + drive->prepareDrive(dir, STRIKER_VEL, tilt()); // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); // else roller->speed(roller->MIN); @@ -89,6 +86,5 @@ void Striker::striker() int Striker::tilt() { - // return CURRENT_DATA_READ.ballAngle <= 110 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0; - return CURRENT_DATA_READ.angleAtkFix; + return CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.atkGAngle_fix : 0; } \ No newline at end of file diff --git a/src/system/lines/linesys_camera.cpp b/src/system/lines/linesys_camera.cpp index b2b1e18..3922064 100644 --- a/src/system/lines/linesys_camera.cpp +++ b/src/system/lines/linesys_camera.cpp @@ -31,7 +31,7 @@ LineSysCamera::LineSysCamera(vector in_, vector out_ void LineSysCamera ::update() { - CURRENT_INPUT_WRITE.lineByte = 0; + CURRENT_INPUT_WRITE.lineByte = 0; inV = 0; outV = 0; tookLine = false; @@ -53,7 +53,7 @@ void LineSysCamera ::update() i = it - out.begin(); ds = *it; linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; - CURRENT_INPUT_WRITE.lineByte |= linetriggerO[i] << (4+i); + CURRENT_INPUT_WRITE.lineByte |= linetriggerO[i] << (4 + i); } for (int i = 0; i < 4; i++) @@ -99,8 +99,18 @@ void LineSysCamera::outOfBounds() if (millis() - exitTimer < EXIT_TIME) { - CURRENT_DATA_WRITE.game->ps->goCenter(); - tookLine = true; + int yangle = CURRENT_DATA_READ.yangle_fix * CURRENT_DATA_READ.ySeen; + int bangle = CURRENT_DATA_READ.bangle_fix * CURRENT_DATA_READ.bSeen; + + int diffB = abs(min(yangle, bangle) - max(yangle, bangle)); + int diffB1 = 360 - diffB; + int diff = min(diffB, diffB1); + + DEBUG.println("AngleY " + String(yangle) + " AngleB" + String(bangle) + " Dir " + String(min(yangle, bangle) + diff)); + + drive->prepareDrive(min(yangle, bangle) + diff, CURRENT_DATA_READ.ySeen || CURRENT_DATA_READ.bSeen ? MAX_VEL : 0, CURRENT_DATA_WRITE.tilt); + // CURRENT_DATA_WRITE.game->ps->goCenter(); + // tookLine = true; tone(BUZZER, 220.00, 250); } else diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 83580ce..a12fcf0 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -13,8 +13,6 @@ int diff(int a, int b){ } PositionSysCamera::PositionSysCamera() { - MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y); - Inputx = 0; Outputx = 0; Setpointx = 0; @@ -29,11 +27,11 @@ PositionSysCamera::PositionSysCamera() { givenMovement = false; X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); - X->SetOutputLimits(-MAX_X, MAX_X); + X->SetOutputLimits(-DIM_X_HALF,DIM_X_HALF); X->SetMode(AUTOMATIC); X->SetSampleTime(2); Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); - Y->SetOutputLimits(-MAX_Y,MAX_Y); + Y->SetOutputLimits(-DIM_Y_HALF, DIM_Y_HALF); Y->SetMode(AUTOMATIC); Y->SetSampleTime(2); @@ -43,30 +41,65 @@ PositionSysCamera::PositionSysCamera() { void PositionSysCamera::update(){ int posx = 0, posy = 0; + CURRENT_DATA_WRITE.camera_back_in_time = false; //Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync //Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot - if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){ - posx = (CURRENT_DATA_WRITE.cam_xy_fixed + CURRENT_DATA_WRITE.cam_xb_fixed) / 2; - posy = CURRENT_DATA_WRITE.cam_yb_fixed + CURRENT_DATA_WRITE.cam_yy_fixed; - //IMPORTANT STEP: or the direction of the plane will be flipped - // posx *= -1; - posy *= -1; - }else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){ - posx = CURRENT_DATA_WRITE.cam_xb_fixed; - posy = CURRENT_DATA_WRITE.cam_yb_fixed + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb_fixed); - //IMPORTANT STEP: or the direction of the plane will be flipped - // posx *= -1; - posy *= -1; - }else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){ - posx = CURRENT_DATA_WRITE.cam_xy_fixed; - posy = CURRENT_DATA_WRITE.cam_yy_fixed + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy_fixed); - //IMPORTANT STEP: or the direction of the plane will be flipped - // posx *= -1; - posy *= -1; - }else{ - // Go back in time until we found a valid status, when we saw at least one goal + if(CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen){ + //project two lines, from the center of the goals to the robot. The point of intersection of these two lines is the position of the robot + //this doesn't work when the angles have tangents that approach infinity, so filtering for that case is needed + + if(CURRENT_DATA_READ.atkGAngle_fix >= 355 || CURRENT_DATA_READ.atkGAngle_fix <= 5 || CURRENT_DATA_READ.defGAngle_fix >= 175 || CURRENT_DATA_READ.defGAngle_fix <= 185){ + //fallback to a method without tangents + //Extend two vector and find the point where they end, then take the average + method = 1; + int posx1 = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; + int posy1 = CAMERA_GOAL_DEF_Y + sin(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; + int posx2 = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; + int posy2 = CAMERA_GOAL_ATK_Y + sin(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; + + posx = (int) ((posx1-posx2)*0.5); + posy = (int) ((posy1-posy2)*0.5); + + }else{ + //resolved manually and checked with wolfram alpha + //here is the solution https://www.wolframalpha.com/input?i=systems+of+equations+calculator&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation1%22%7D+-%3E%22y-j+%3D+tan%28a%29%28x-i%29%22&assumption=%22FSelect%22+-%3E+%7B%7B%22SolveSystemOf2EquationsCalculator%22%7D%7D&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation2%22%7D+-%3E%22y-v%3Dtan%28b%29%28x-u%29%22 + //(i,j), (u,v) are the coords of the two goals. Some stuff cancels out since we assume that the goals always have 0 as x coord + method = 0; + + float anglea = (90-CURRENT_DATA_READ.atkGAngle_fix+360)%360; + float angleb = (270-CURRENT_DATA_READ.defGAngle_fix+360)%360; + + float tana = tan(radians(anglea)); + float tanb = tan(radians(angleb)); + + float tanb_tana_diff = tanb - tana; + + float posx_n = -CAMERA_GOAL_DEF_Y + CAMERA_GOAL_ATK_Y; + float posy_n = CAMERA_GOAL_ATK_Y*tanb + CAMERA_GOAL_DEF_Y*tana; + + posx = (int) (posx_n/tanb_tana_diff); + posy = (int) (posy_n/tanb_tana_diff); + } + + + }else if (!CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen){ + method = 2; + + //Extend a vector from a known point and reach the position of the robot + posx = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; + posy = CAMERA_GOAL_DEF_Y + sin(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; + }else if (CURRENT_DATA_WRITE.atkSeen && !CURRENT_DATA_WRITE.defSeen == true){ + method = 3; + + //Extend a vector from a known point and reach the position of the robot + posx = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; + posy = CAMERA_GOAL_ATK_Y + sin(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; + }else{ + method = 4; + + // Go back in time until we found a valid status, when we saw at least one goal for(int i = 1; i < dim; i++){ valid_data = getDataAtIndex_backwardsFromCurrent(i); if(valid_data.ySeen || valid_data.bSeen){ @@ -76,6 +109,8 @@ 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; + CURRENT_DATA_WRITE.camera_back_in_time = true; + break; } } @@ -84,9 +119,9 @@ void PositionSysCamera::update(){ CURRENT_DATA_WRITE.posx = posx; CURRENT_DATA_WRITE.posy = posy; + Inputx = posx; Inputy = posy; - //Prepare for receiving information about movement //Starting setpoint position as current position Setpointx = posx; @@ -126,11 +161,6 @@ 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; -} bool PositionSysCamera::isInTheVicinityOf(int x_, int y_){ // Distance using pytagorean theorem @@ -181,7 +211,7 @@ void PositionSysCamera::CameraPID(){ CURRENT_DATA_WRITE.addvx = vx; CURRENT_DATA_WRITE.addvy = vy; #else - int tmp = (CURRENT_DATA_READ.tilt+360)%360; + int tmp = (CURRENT_DATA_WRITE.tilt+360)%360; dir = dir-tmp; if(dir < 0) dir+=360; drive->prepareDrive(dir , speed, CURRENT_DATA_WRITE.tilt); @@ -192,6 +222,6 @@ void PositionSysCamera::CameraPID(){ } } - void PositionSysCamera::test(){ + DEBUG.println("Using method " + String(method) + " Position: (" + String(CURRENT_DATA_WRITE.posx) + ", " + String(CURRENT_DATA_WRITE.posy) + ")" ); } diff --git a/src/system/positions/positionsys_zone.cpp b/src/system/positions/positionsys_zone.cpp index 68fe989..53706ee 100644 --- a/src/system/positions/positionsys_zone.cpp +++ b/src/system/positions/positionsys_zone.cpp @@ -263,9 +263,9 @@ void PositionSysZone::phyZoneUS(){ void PositionSysZone::phyZoneCam(){ //IMU-fixed attack angle - camA = CURRENT_DATA_READ.angleAtkFix; + camA = CURRENT_DATA_READ.atkGAngle_fix; //IMU-fixed defence angle - camD = CURRENT_DATA_READ.angleDefFix; + camD = CURRENT_DATA_READ.defGAngle_fix; //Negative angle means that the robot is positioned to the right of the goalpost //Positive angle means that the robot is positioned to the left of the goalpost diff --git a/src/test_menu.cpp b/src/test_menu.cpp index d7dedca..4e190be 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -83,7 +83,7 @@ void TestMenu::testMenu() case '8': DEBUG.println("Camera tilt Test"); drive->resetDrive(); - drive->prepareDrive(0, 0, CURRENT_DATA_READ.angleAtkFix); + drive->prepareDrive(0, 0, CURRENT_DATA_READ.atkGAngle_fix); break; case '9': DEBUG.println("Switches as seen from behind:"); @@ -96,11 +96,8 @@ void TestMenu::testMenu() case 'p': if(millis() - timer > 150){ timer = millis(); - DEBUG.println("Position on the field (camera)"); - DEBUG.print("X: "); - DEBUG.print(CURRENT_DATA_READ.posx); - DEBUG.print(" , Y: "); - DEBUG.println(CURRENT_DATA_READ.posy); + DEBUG.println("Position on the field"); + CURRENT_DATA_WRITE.game->ps->test(); } break; case 'u': diff --git a/utility/OpenMV/conic_angle_dist.py b/utility/OpenMV/conic_angle_dist.py new file mode 100644 index 0000000..81702e8 --- /dev/null +++ b/utility/OpenMV/conic_angle_dist.py @@ -0,0 +1,153 @@ +# color tracking with conic mirror - By: EmaMaker - wed 15 jan 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) + +CHR_UNKNOWN = '999' + +y_found = False +b_found = False + +#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/ +def val_map(x, in_min, in_max, out_min, out_max): + x = int(x) + in_min = int(in_min) + in_max = int(in_max) + out_min = int(out_min) + out_max = int(out_max) + return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) + +# Check side +def isInLeftSide(img, x): + return x < img.height() / 2 + +def isInRightSide(img, x): + return x >= img.height() / 2 + + +# 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 = [ (72, 100, -26, 12, 37, 91), # thresholds yellow goalz + (45, 70, -9, 29, -80, -42)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + + +roi = (40, 0, 260, 240) + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QVGA) +sensor.set_windowing(roi) +sensor.set_contrast(3) +sensor.set_saturation(3) +sensor.set_brightness(3) +sensor.set_auto_whitebal(False, (-6.02073, -5.494869, -0.8559153)) +sensor.set_auto_exposure(False, 5845) +#sensor.set_auto_gain(True) +sensor.skip_frames(time = 300) + +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())) + + blue_led.off() + + y_found = False + b_found = False + + 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=100, area_threshold=100, 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() ) ] + y_found = True + if (blob.code() == 2): + tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] + b_found = True + + 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) + + #Compute everything related to Yellow First + y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1] + y_cx = int(y1_cx - img.width() / 2) + y_cy = int(img.height() / 2 - y1_cy) + + #For simplicity's sake, normalize data between 0 and 99 (100 values in total) + if y_found == True: + img.draw_cross(y1_cx, y1_cy) + + #y_cy = val_map(y_cx, -img.height() / 2, img.height() / 2, 99, 0) + #y_cx = val_map(y_cy, -img.width() / 2, img.width() / 2, 0, 99) + + #compute angle + y_angle = atan2(y_cy, y_cx) + y_angle = -90 + (y_angle * 180 / 3.14) #convert to degrees and shift + y_angle = (y_angle+360)%360 #make sure it's all positive, so we don't have strange things happening when receiving on teensy's side + + #compute dist + y_dist = sqrt(y_cx*y_cx + y_cy*y_cy) + + data = "{}{}{}{}{}".format("Y", str(y_angle), "-", str(y_dist), "y") + # il numero 999 indica che non ho trovato nessun blob + else: + data = "{}{}{}{}{}".format("Y", "999", "-", "0", "y") + + + # trasmetto dati in seriale e test su terminale + uart.write(data) + + + b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1] + b_cx = int(b1_cx - img.width() / 2) + b_cy = int(img.height() / 2 - b1_cy) + + if b_found == True: + img.draw_cross(b1_cx, b1_cy) + + #b_cy = val_map(b_cx, -img.height() / 2, img.height() / 2, 99, 0) + #b_cx = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 99) + + #compute angle + b_angle = atan2(y_cy, y_cx) #to fix to account for camera and robot rotation + b_angle = -90 + (b_angle * 180 / 3.14) #convert to degrees and shift + b_angle = (b_angle+360)%360 #make sure it's all positive, so we don't have strange things happening when receiving on teensy's side + #compute dist + b_dist = sqrt(y_cx*y_cx + y_cy*y_cy) + + data = "{}{}{}{}{}".format("B", str(b_angle), "-", str(b_dist), "b") + # il numero 999 indica che non ho trovato nessun blob + else: + data = "{}{}{}{}{}".format("B", "999", "-", "0", "b") + + + #BLUE FIRST, YELLOW SECOND. ANGLE FIRST, DISTANCE SECOND + print(str(s_b_angle) + " | " + str(s_b_dist) + " --- " + str(s_y_angle) + " | " + str(s_y_dist)) From 4218ac67b501599ef7cc3aff7df15e7b3c7e5188 Mon Sep 17 00:00:00 2001 From: emamaker Date: Mon, 4 Jul 2022 17:54:56 +0200 Subject: [PATCH 05/14] camera: compute data after receiving --- .../data_source_camera_conicmirror.cpp | 84 +++++++++++-------- 1 file changed, 47 insertions(+), 37 deletions(-) diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index 3475510..79feccb 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -17,49 +17,59 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D void DataSourceCameraConic ::readSensor() { - while (ser->available() > 0) + while (ser->available()) { current_char = ser->read(); - //update status vector + // update status vector CURRENT_INPUT_WRITE.cameraChar = current_char; // start if (current_char == 'B' || current_char == 'Y') { - start_char = current_char; - start = true; - end = false; - dash = false; + start_char = current_char; + start = true; + end = false; + dash = false; + s1 = ""; + s2 = ""; } // end else if (current_char == 'b' || current_char == 'y') { - end_char = current_char; + end_char = current_char; - if(start && dash && ((start_char == 'B' && end_char == 'b') || (start_char == 'Y' && end_char == 'y') ) ){ - if(start_char == 'B'){ - bangle = s1.toInt(); - bdist = s2.toInt(); - }else{ - yangle = s1.toInt(); - ydist = s2.toInt(); + if (start && dash && ((start_char == 'B' && end_char == 'b') || (start_char == 'Y' && end_char == 'y'))) + { + + if (start_char == 'B') + { + bangle = s1.toInt(); + bdist = s2.toInt(); + } + else + { + yangle = s1.toInt(); + ydist = s2.toInt(); + } + computeCoordsAngles(); } - } - end = true; - start = false; - dash = false; - count = 0; - }// dash - else if (current_char == '-') + end = true; + start = false; + dash = false; + count = 0; + } // dash + else if (current_char == '-' && start) { - dash = true; + dash = true; } // it's a number - else if (isDigit(current_char)) + else if (isDigit(current_char) && start) { - if(dash) s2 += current_char; - else s1 += current_char; + if (dash) + s2 += current_char; + else + s1 += current_char; } } } @@ -68,20 +78,20 @@ void DataSourceCameraConic ::readSensor() void DataSourceCameraConic ::computeCoordsAngles() { - #ifdef CAMERA_CONIC_FILTER_POINTS - yangle = filt_yangle->calculate(yangle); - ydist = filt_ydist->calculate(ydist); - bangle = filt_bangle->calculate(bangle); - bdist = filt_bdist->calculate(bdist); - #endif +#ifdef CAMERA_CONIC_FILTER_POINTS + yangle = filt_yangle->calculate(yangle); + ydist = filt_ydist->calculate(ydist); + bangle = filt_bangle->calculate(bangle); + bdist = filt_bdist->calculate(bdist); +#endif // Fix angles using the IMU int tmp = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; - yangle_fix = (yangle+tmp+360) % 360; - bangle_fix = (bangle+tmp+360) % 360; + yangle_fix = (yangle + tmp + 360) % 360; + bangle_fix = (bangle + tmp + 360) % 360; - //TODO: Maybe add a complementary filter on fixed angles ? + // TODO: Maybe add a complementary filter on fixed angles ? // Important: update status vector CURRENT_DATA_WRITE.yangle = yangle; @@ -106,7 +116,7 @@ void DataSourceCameraConic ::computeCoordsAngles() void DataSourceCameraConic::test() { - DEBUG.print("BLUE GOAL:: Seen: " + String(CURRENT_DATA_READ.bSeen) + " | Angle: " + CURRENT_DATA_READ.bangle + " | Fixed Angle: " + CURRENT_DATA_READ.bangle_fix + " | Dist: " + CURRENT_DATA_READ.bdist); - DEBUG.print("YELLOW GOAL:: Seen: " + String(CURRENT_DATA_READ.ySeen) + " | Angle: " + CURRENT_DATA_READ.yangle + " | Fixed Angle: " + CURRENT_DATA_READ.yangle_fix + " | Dist: " + CURRENT_DATA_READ.ydist); - delay(150); + DEBUG.println("Received char '" + String(CURRENT_INPUT_READ.cameraChar) + "'"); + DEBUG.println("BLUE GOAL:: Seen: " + String(CURRENT_DATA_READ.bSeen) + " | Angle: " + CURRENT_DATA_READ.bangle + " | Fixed Angle: " + CURRENT_DATA_READ.bangle_fix + " | Dist: " + CURRENT_DATA_READ.bdist); + DEBUG.println("YELLOW GOAL:: Seen: " + String(CURRENT_DATA_READ.ySeen) + " | Angle: " + CURRENT_DATA_READ.yangle + " | Fixed Angle: " + CURRENT_DATA_READ.yangle_fix + " | Dist: " + CURRENT_DATA_READ.ydist); } From f1fab59b2a98e94df84d8363aea4513ee063a432 Mon Sep 17 00:00:00 2001 From: emamaker Date: Mon, 4 Jul 2022 17:55:47 +0200 Subject: [PATCH 06/14] cam-py: correct calc and send data for blue goal --- utility/OpenMV/conic_angle_dist.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/utility/OpenMV/conic_angle_dist.py b/utility/OpenMV/conic_angle_dist.py index 81702e8..4bfd115 100644 --- a/utility/OpenMV/conic_angle_dist.py +++ b/utility/OpenMV/conic_angle_dist.py @@ -109,21 +109,21 @@ while(True): #y_cx = val_map(y_cy, -img.width() / 2, img.width() / 2, 0, 99) #compute angle - y_angle = atan2(y_cy, y_cx) + y_angle = math.atan2(y_cy, y_cx) y_angle = -90 + (y_angle * 180 / 3.14) #convert to degrees and shift - y_angle = (y_angle+360)%360 #make sure it's all positive, so we don't have strange things happening when receiving on teensy's side + y_angle = (int(y_angle)+360)%360 #make sure it's all positive, so we don't have strange things happening when receiving on teensy's side #compute dist - y_dist = sqrt(y_cx*y_cx + y_cy*y_cy) + y_dist = int(math.sqrt(y_cx*y_cx + y_cy*y_cy)) - data = "{}{}{}{}{}".format("Y", str(y_angle), "-", str(y_dist), "y") + ydata = "{}{}{}{}{}".format("Y", str(y_angle), "-", str(y_dist), "y") # il numero 999 indica che non ho trovato nessun blob else: - data = "{}{}{}{}{}".format("Y", "999", "-", "0", "y") + ydata = "{}{}{}{}{}".format("Y", "999", "-", "0", "y") # trasmetto dati in seriale e test su terminale - uart.write(data) + uart.write(ydata) b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1] @@ -137,17 +137,21 @@ while(True): #b_cx = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 99) #compute angle - b_angle = atan2(y_cy, y_cx) #to fix to account for camera and robot rotation + b_angle = math.atan2(b_cy, b_cx) #to fix to account for camera and robot rotation b_angle = -90 + (b_angle * 180 / 3.14) #convert to degrees and shift - b_angle = (b_angle+360)%360 #make sure it's all positive, so we don't have strange things happening when receiving on teensy's side + b_angle = (int(b_angle)+360)%360 #make sure it's all positive, so we don't have strange things happening when receiving on teensy's side #compute dist - b_dist = sqrt(y_cx*y_cx + y_cy*y_cy) + b_dist = int(math.sqrt(b_cx*b_cx + b_cy*b_cy)) - data = "{}{}{}{}{}".format("B", str(b_angle), "-", str(b_dist), "b") + bdata = "{}{}{}{}{}".format("B", str(b_angle), "-", str(b_dist), "b") # il numero 999 indica che non ho trovato nessun blob else: - data = "{}{}{}{}{}".format("B", "999", "-", "0", "b") + bdata = "{}{}{}{}{}".format("B", "999", "-", "0", "b") + + uart.write(bdata) #BLUE FIRST, YELLOW SECOND. ANGLE FIRST, DISTANCE SECOND - print(str(s_b_angle) + " | " + str(s_b_dist) + " --- " + str(s_y_angle) + " | " + str(s_y_dist)) + #print(str(b_angle) + " | " + str(b_dist) + " --- " + str(y_angle) + " | " + str(y_dist)) + print(f"Blue {bdata}") + print(f"Yellow {ydata}") From ba0b231cd080ec478960a7dd89c6d0b3e3b18b9f Mon Sep 17 00:00:00 2001 From: emamaker Date: Mon, 4 Jul 2022 17:56:15 +0200 Subject: [PATCH 07/14] pos-sys-cam: vector calc should occur in radians --- src/system/positions/positionsys_camera.cpp | 219 +++++++++++--------- 1 file changed, 119 insertions(+), 100 deletions(-) diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index a12fcf0..9036e3c 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -1,18 +1,19 @@ -#include "behaviour_control/status_vector.h" #include "systems/position/positionsys_camera.h" +#include "behaviour_control/status_vector.h" +#include "math.h" #include "sensors/sensors.h" #include "vars.h" -#include "math.h" - -int diff(int a, int b){ - int diffB = abs(min(a, b) - max(a, b)); - int diffB1 = 360-diffB; - int diff = min(diffB, diffB1); - return diff; +int diff(int a, int b) +{ + int diffB = abs(min(a, b) - max(a, b)); + int diffB1 = 360 - diffB; + int diff = min(diffB, diffB1); + return diff; } -PositionSysCamera::PositionSysCamera() { +PositionSysCamera::PositionSysCamera() +{ Inputx = 0; Outputx = 0; Setpointx = 0; @@ -25,9 +26,9 @@ PositionSysCamera::PositionSysCamera() { axisx = 0; axisy = 0; givenMovement = false; - + X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); - X->SetOutputLimits(-DIM_X_HALF,DIM_X_HALF); + X->SetOutputLimits(-DIM_X_HALF, DIM_X_HALF); X->SetMode(AUTOMATIC); X->SetSampleTime(2); Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); @@ -39,37 +40,41 @@ PositionSysCamera::PositionSysCamera() { filterSpeed = new ComplementaryFilter(0.65); } -void PositionSysCamera::update(){ +void PositionSysCamera::update() +{ int posx = 0, posy = 0; CURRENT_DATA_WRITE.camera_back_in_time = false; - //Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync - //Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot - if(CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen){ - //project two lines, from the center of the goals to the robot. The point of intersection of these two lines is the position of the robot - //this doesn't work when the angles have tangents that approach infinity, so filtering for that case is needed - - if(CURRENT_DATA_READ.atkGAngle_fix >= 355 || CURRENT_DATA_READ.atkGAngle_fix <= 5 || CURRENT_DATA_READ.defGAngle_fix >= 175 || CURRENT_DATA_READ.defGAngle_fix <= 185){ - //fallback to a method without tangents - //Extend two vector and find the point where they end, then take the average + // Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync + // Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot + if (CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen) + { + // project two lines, from the center of the goals to the robot. The point of intersection of these two lines is the position of the robot + // this doesn't work when the angles have tangents that approach infinity, so filtering for that case is needed + + if (CURRENT_DATA_READ.atkGAngle_fix == 0 || CURRENT_DATA_READ.atkGAngle_fix == 180 || CURRENT_DATA_READ.defGAngle_fix == 0 || CURRENT_DATA_READ.defGAngle_fix == 180) + { + // fallback to a method without tangents + // Extend two vector and find the point where they end, then take the average method = 1; - int posx1 = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; - int posy1 = CAMERA_GOAL_DEF_Y + sin(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; - int posx2 = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; - int posy2 = CAMERA_GOAL_ATK_Y + sin(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; + int posx1 = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; + int posy1 = CAMERA_GOAL_DEF_Y + sin(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; + int posx2 = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; + int posy2 = CAMERA_GOAL_ATK_Y + sin(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; - posx = (int) ((posx1-posx2)*0.5); - posy = (int) ((posy1-posy2)*0.5); - - }else{ - //resolved manually and checked with wolfram alpha - //here is the solution https://www.wolframalpha.com/input?i=systems+of+equations+calculator&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation1%22%7D+-%3E%22y-j+%3D+tan%28a%29%28x-i%29%22&assumption=%22FSelect%22+-%3E+%7B%7B%22SolveSystemOf2EquationsCalculator%22%7D%7D&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation2%22%7D+-%3E%22y-v%3Dtan%28b%29%28x-u%29%22 + posx = (int)((posx1 + posx2) * 0.5); + posy = (int)((posy1 + posy2) * 0.5); + } + else + { + // resolved manually and checked with wolfram alpha + // here is the solution https://www.wolframalpha.com/input?i=systems+of+equations+calculator&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation1%22%7D+-%3E%22y-j+%3D+tan%28a%29%28x-i%29%22&assumption=%22FSelect%22+-%3E+%7B%7B%22SolveSystemOf2EquationsCalculator%22%7D%7D&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation2%22%7D+-%3E%22y-v%3Dtan%28b%29%28x-u%29%22 //(i,j), (u,v) are the coords of the two goals. Some stuff cancels out since we assume that the goals always have 0 as x coord method = 0; - float anglea = (90-CURRENT_DATA_READ.atkGAngle_fix+360)%360; - float angleb = (270-CURRENT_DATA_READ.defGAngle_fix+360)%360; + float anglea = (90 - CURRENT_DATA_READ.atkGAngle_fix + 360) % 360; + float angleb = (270 - CURRENT_DATA_READ.defGAngle_fix + 360) % 360; float tana = tan(radians(anglea)); float tanb = tan(radians(angleb)); @@ -77,32 +82,38 @@ void PositionSysCamera::update(){ float tanb_tana_diff = tanb - tana; float posx_n = -CAMERA_GOAL_DEF_Y + CAMERA_GOAL_ATK_Y; - float posy_n = CAMERA_GOAL_ATK_Y*tanb + CAMERA_GOAL_DEF_Y*tana; + float posy_n = CAMERA_GOAL_ATK_Y * tanb + CAMERA_GOAL_DEF_Y * tana; - posx = (int) (posx_n/tanb_tana_diff); - posy = (int) (posy_n/tanb_tana_diff); + posx = (int)(posx_n / tanb_tana_diff); + posy = (int)(posy_n / tanb_tana_diff); } - - - }else if (!CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen){ + } + else if (!CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen) + { method = 2; - - //Extend a vector from a known point and reach the position of the robot - posx = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; - posy = CAMERA_GOAL_DEF_Y + sin(CURRENT_DATA_READ.defGAngle_fix)*CURRENT_DATA_READ.defGDist; - }else if (CURRENT_DATA_WRITE.atkSeen && !CURRENT_DATA_WRITE.defSeen == true){ + + // Extend a vector from a known point and reach the position of the robot + posx = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; + posy = CAMERA_GOAL_DEF_Y + sin(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; + } + else if (CURRENT_DATA_WRITE.atkSeen && !CURRENT_DATA_WRITE.defSeen == true) + { method = 3; - - //Extend a vector from a known point and reach the position of the robot - posx = CAMERA_GOAL_X + cos(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; - posy = CAMERA_GOAL_ATK_Y + sin(CURRENT_DATA_READ.atkGAngle_fix)*CURRENT_DATA_READ.atkGDist; - }else{ + + // Extend a vector from a known point and reach the position of the robot + posx = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; + posy = CAMERA_GOAL_ATK_Y + sin(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; + } + else + { method = 4; - + // Go back in time until we found a valid status, when we saw at least one goal - for(int i = 1; i < dim; i++){ + for (int i = 1; i < dim; i++) + { valid_data = getDataAtIndex_backwardsFromCurrent(i); - if(valid_data.ySeen || valid_data.bSeen){ + if (valid_data.ySeen || valid_data.bSeen) + { posx = valid_data.posx; posy = valid_data.posy; @@ -114,16 +125,15 @@ void PositionSysCamera::update(){ break; } } - } CURRENT_DATA_WRITE.posx = posx; CURRENT_DATA_WRITE.posy = posy; - + Inputx = posx; Inputy = posy; - //Prepare for receiving information about movement - //Starting setpoint position as current position + // Prepare for receiving information about movement + // Starting setpoint position as current position Setpointx = posx; Setpointy = posy; axisx = 0; @@ -131,8 +141,9 @@ void PositionSysCamera::update(){ givenMovement = false; } -//This means the last time this is called has the biggest priority, has for prepareDrive -void PositionSysCamera::setMoveSetpoints(int x, int y){ +// This means the last time this is called has the biggest priority, has for prepareDrive +void PositionSysCamera::setMoveSetpoints(int x, int y) +{ // Setpointx = x + CAMERA_TRANSLATION_X; // Setpointy = y + CAMERA_TRANSLATION_Y; Setpointx = x; @@ -141,18 +152,21 @@ void PositionSysCamera::setMoveSetpoints(int x, int y){ CameraPID(); } -void PositionSysCamera::addMoveOnAxis(int x, int y){ +void PositionSysCamera::addMoveOnAxis(int x, int y) +{ axisx += x; axisy += y; givenMovement = true; CameraPID(); } -void PositionSysCamera::goCenter(){ +void PositionSysCamera::goCenter() +{ setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); } -void PositionSysCamera::centerGoal(){ +void PositionSysCamera::centerGoal() +{ setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y); } @@ -162,23 +176,28 @@ By subtracting the absolute value of the goal y we know to the sum of the absolu The sign of the goal y we found is simply the reverse of the one we got */ -bool PositionSysCamera::isInTheVicinityOf(int x_, int y_){ +bool PositionSysCamera::isInTheVicinityOf(int x_, int y_) +{ // Distance using pytagorean theorem - return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= VICINITY_DIST_TRESH*VICINITY_DIST_TRESH; + return pow(CURRENT_DATA_READ.posx - x_, 2) + pow(CURRENT_DATA_READ.posy - y_, 2) <= VICINITY_DIST_TRESH * VICINITY_DIST_TRESH; } -bool PositionSysCamera::isInRoughVicinityOf(int x_, int y_){ +bool PositionSysCamera::isInRoughVicinityOf(int x_, int y_) +{ // Distance using pytagorean theorem - return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= ROUGH_VICINITY_DIST_TRESH*ROUGH_VICINITY_DIST_TRESH; + return pow(CURRENT_DATA_READ.posx - x_, 2) + pow(CURRENT_DATA_READ.posy - y_, 2) <= ROUGH_VICINITY_DIST_TRESH * ROUGH_VICINITY_DIST_TRESH; } -bool PositionSysCamera::isAtDistanceFrom(int x_, int y_, int dist){ +bool PositionSysCamera::isAtDistanceFrom(int x_, int y_, int dist) +{ // Distance using pytagorean theorem - return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= dist*dist; + return pow(CURRENT_DATA_READ.posx - x_, 2) + pow(CURRENT_DATA_READ.posy - y_, 2) <= dist * dist; } -void PositionSysCamera::CameraPID(){ - if(givenMovement){ +void PositionSysCamera::CameraPID() +{ + if (givenMovement) + { vx = 0; vy = 0; @@ -186,42 +205,42 @@ void PositionSysCamera::CameraPID(){ Setpointx += axisx; Setpointy += axisy; - if(X->Compute() && Y->Compute()){ - - //Compute an X and Y to give to the PID later - //There's surely a better way to do this - int dir = -90-(atan2(Outputy,Outputx)*180/3.14); - dir = (dir+360) % 360; + if (X->Compute() && Y->Compute()) + { - float distance = hypot(Outputx, Outputy); - float speed = distance > 2 ? 35 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_VEL) : 0; + // Compute an X and Y to give to the PID later + // There's surely a better way to do this + int dir = -90 - (atan2(Outputy, Outputx) * 180 / 3.14); + dir = (dir + 360) % 360; - // DEBUG.print("x: "); - // DEBUG.print(Outputx); - // DEBUG.print(" y:"); - // DEBUG.print(Outputy); - // DEBUG.print(" Hypot:"); - // DEBUG.print(hypot(Outputx, Outputy)); - // DEBUG.print(" Speed:"); - // DEBUG.println(speed); + float distance = hypot(Outputx, Outputy); + float speed = distance > 2 ? 35 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_VEL) : 0; - #ifdef DRIVE_VECTOR_SUM - vx = ((speed * cosins[dir])); - vy = ((-speed * sins[dir])); - CURRENT_DATA_WRITE.addvx = vx; - CURRENT_DATA_WRITE.addvy = vy; - #else - int tmp = (CURRENT_DATA_WRITE.tilt+360)%360; - dir = dir-tmp; - if(dir < 0) dir+=360; - drive->prepareDrive(dir , speed, CURRENT_DATA_WRITE.tilt); - #endif - + // DEBUG.print("x: "); + // DEBUG.print(Outputx); + // DEBUG.print(" y:"); + // DEBUG.print(Outputy); + // DEBUG.print(" Hypot:"); + // DEBUG.print(hypot(Outputx, Outputy)); + // DEBUG.print(" Speed:"); + // DEBUG.println(speed); + +#ifdef DRIVE_VECTOR_SUM + vx = ((speed * cosins[dir])); + vy = ((-speed * sins[dir])); + CURRENT_DATA_WRITE.addvx = vx; + CURRENT_DATA_WRITE.addvy = vy; +#else + int tmp = (CURRENT_DATA_WRITE.tilt + 360) % 360; + dir = dir - tmp; + if (dir < 0) dir += 360; + drive->prepareDrive(dir, speed, CURRENT_DATA_WRITE.tilt); +#endif } - } } -void PositionSysCamera::test(){ - DEBUG.println("Using method " + String(method) + " Position: (" + String(CURRENT_DATA_WRITE.posx) + ", " + String(CURRENT_DATA_WRITE.posy) + ")" ); +void PositionSysCamera::test() +{ + DEBUG.println("Using method " + String(method) + " Position: (" + String(CURRENT_DATA_WRITE.posx) + ", " + String(CURRENT_DATA_WRITE.posy) + ")"); } From d4bc1cf90480c6c8c120a066890a7d5dbba79df6 Mon Sep 17 00:00:00 2001 From: emamaker Date: Tue, 5 Jul 2022 12:01:19 +0200 Subject: [PATCH 08/14] camera: harder filter --- include/sensors/data_source_camera_conicmirror.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index a98a1b8..49a30f1 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -5,7 +5,7 @@ #include "behaviour_control/complementary_filter.h" -#define FILTER_DEFAULT_COEFF 0.6 +#define FILTER_DEFAULT_COEFF 0.85 #define FILTER_YANGLE_COEFF FILTER_DEFAULT_COEFF #define FILTER_BANGLE_COEFF FILTER_DEFAULT_COEFF #define FILTER_YANGLE_FIX_COEFF FILTER_DEFAULT_COEFF From 8beec8c246dfb21bcd768f589864e464f64b2db3 Mon Sep 17 00:00:00 2001 From: emamaker Date: Tue, 5 Jul 2022 12:01:40 +0200 Subject: [PATCH 09/14] camera: only filter if seen --- .../data_source_camera_conicmirror.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index 79feccb..dd1d320 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -78,11 +78,20 @@ void DataSourceCameraConic ::readSensor() void DataSourceCameraConic ::computeCoordsAngles() { + CURRENT_DATA_WRITE.bSeen = bangle != 999; + CURRENT_DATA_WRITE.ySeen = yangle != 999; + #ifdef CAMERA_CONIC_FILTER_POINTS - yangle = filt_yangle->calculate(yangle); - ydist = filt_ydist->calculate(ydist); - bangle = filt_bangle->calculate(bangle); - bdist = filt_bdist->calculate(bdist); + if (CURRENT_DATA_WRITE.ySeen) + { + yangle = filt_yangle->calculate(yangle); + ydist = filt_ydist->calculate(ydist); + } + if (CURRENT_DATA_WRITE.bSeen) + { + bangle = filt_bangle->calculate(bangle); + bdist = filt_bdist->calculate(bdist); + } #endif // Fix angles using the IMU @@ -100,8 +109,6 @@ void DataSourceCameraConic ::computeCoordsAngles() CURRENT_DATA_WRITE.bangle_fix = bangle_fix; CURRENT_DATA_WRITE.ydist = ydist; CURRENT_DATA_WRITE.bdist = bdist; - CURRENT_DATA_WRITE.bSeen = bangle != 999; - CURRENT_DATA_WRITE.ySeen = yangle != 999; CURRENT_DATA_WRITE.atkGAngle = goalOrientation ? yangle : bangle; CURRENT_DATA_WRITE.atkGAngle_fix = goalOrientation ? yangle_fix : bangle_fix; From eb9e8ad4754ec3ae63c33ae0ec5289320ddcf1d0 Mon Sep 17 00:00:00 2001 From: emamaker Date: Tue, 5 Jul 2022 12:02:18 +0200 Subject: [PATCH 10/14] pos-sys-cam: correct calculations --- include/systems/position/positionsys_camera.h | 8 ++-- src/system/positions/positionsys_camera.cpp | 48 +++++++++++-------- 2 files changed, 32 insertions(+), 24 deletions(-) diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index b402634..3f6fc82 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -13,10 +13,10 @@ #define CAMERA_GOAL_MIN_X (-8) //dimensions of the field, for how we scale it -#define DIM_X 50 -#define DIM_X_HALF 25 -#define DIM_Y 80 -#define DIM_Y_HALF 40 +#define DIM_X 220 +#define DIM_X_HALF 110 +#define DIM_Y 240 +#define DIM_Y_HALF 120 //where is the center of a goal blob as seen by openmv on the field. For atk goal it's positive, for def goal it's negative #define CAMERA_GOAL_X 0 diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 9036e3c..4fb8cab 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -45,6 +45,14 @@ void PositionSysCamera::update() int posx = 0, posy = 0; CURRENT_DATA_WRITE.camera_back_in_time = false; + double anglea = (double)((90 - CURRENT_DATA_READ.atkGAngle_fix + 360) % 360); + double angled = (double)((270 - CURRENT_DATA_READ.defGAngle_fix + 360) % 360); + + double anglea_rad = radians(anglea); + double angled_rad = radians(angled); + + // DEBUG.println("Angles from goals " + String(anglea) + ", " + String(angled)); + // Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync // Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot if (CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen) @@ -52,16 +60,19 @@ void PositionSysCamera::update() // project two lines, from the center of the goals to the robot. The point of intersection of these two lines is the position of the robot // this doesn't work when the angles have tangents that approach infinity, so filtering for that case is needed - if (CURRENT_DATA_READ.atkGAngle_fix == 0 || CURRENT_DATA_READ.atkGAngle_fix == 180 || CURRENT_DATA_READ.defGAngle_fix == 0 || CURRENT_DATA_READ.defGAngle_fix == 180) + if (CURRENT_DATA_READ.atkGAngle_fix >= 355 || CURRENT_DATA_READ.atkGAngle_fix <= 5 || (CURRENT_DATA_READ.defGAngle_fix >= 175 && CURRENT_DATA_READ.defGAngle_fix <= 185)) { // fallback to a method without tangents // Extend two vector and find the point where they end, then take the average method = 1; - int posx1 = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; - int posy1 = CAMERA_GOAL_DEF_Y + sin(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; - int posx2 = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; - int posy2 = CAMERA_GOAL_ATK_Y + sin(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; + int posx1 = (int)(cos(angled_rad) * CURRENT_DATA_READ.defGDist); + int posy1 = (int)(CAMERA_GOAL_DEF_Y + sin(angled_rad) * CURRENT_DATA_READ.defGDist); + int posx2 = (int)(cos(anglea_rad) * CURRENT_DATA_READ.atkGDist); + int posy2 = (int)(CAMERA_GOAL_ATK_Y - sin(anglea_rad) * CURRENT_DATA_READ.atkGDist); + + // DEBUG.println("POSX1, POSY1 " + String(posx1) + "," + String(posy1)); + // DEBUG.println("POSX2, POSY2 " + String(posx2) + "," + String(posy2)); posx = (int)((posx1 + posx2) * 0.5); posy = (int)((posy1 + posy2) * 0.5); @@ -73,19 +84,16 @@ void PositionSysCamera::update() //(i,j), (u,v) are the coords of the two goals. Some stuff cancels out since we assume that the goals always have 0 as x coord method = 0; - float anglea = (90 - CURRENT_DATA_READ.atkGAngle_fix + 360) % 360; - float angleb = (270 - CURRENT_DATA_READ.defGAngle_fix + 360) % 360; + double tana = tan(anglea_rad); + double tanb = tan(angled_rad); - float tana = tan(radians(anglea)); - float tanb = tan(radians(angleb)); + double tana_tanb_diff = tana - tanb; - float tanb_tana_diff = tanb - tana; + double posx_n = CAMERA_GOAL_DEF_Y - CAMERA_GOAL_ATK_Y; + double posy_n = -CAMERA_GOAL_ATK_Y * tanb + CAMERA_GOAL_DEF_Y * tana; - float posx_n = -CAMERA_GOAL_DEF_Y + CAMERA_GOAL_ATK_Y; - float posy_n = CAMERA_GOAL_ATK_Y * tanb + CAMERA_GOAL_DEF_Y * tana; - - posx = (int)(posx_n / tanb_tana_diff); - posy = (int)(posy_n / tanb_tana_diff); + posx = (int)(posx_n / tana_tanb_diff); + posy = (int)(posy_n / tana_tanb_diff); } } else if (!CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen) @@ -93,16 +101,16 @@ void PositionSysCamera::update() method = 2; // Extend a vector from a known point and reach the position of the robot - posx = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; - posy = CAMERA_GOAL_DEF_Y + sin(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; + posx = CAMERA_GOAL_X + cos(angled_rad) * CURRENT_DATA_READ.defGDist; + posy = CAMERA_GOAL_DEF_Y + sin(angled_rad) * CURRENT_DATA_READ.defGDist; } - else if (CURRENT_DATA_WRITE.atkSeen && !CURRENT_DATA_WRITE.defSeen == true) + else if (CURRENT_DATA_WRITE.atkSeen && !CURRENT_DATA_WRITE.defSeen) { method = 3; // Extend a vector from a known point and reach the position of the robot - posx = CAMERA_GOAL_X + cos(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; - posy = CAMERA_GOAL_ATK_Y + sin(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; + posx = CAMERA_GOAL_X + cos(anglea_rad) * CURRENT_DATA_READ.atkGDist; + posy = CAMERA_GOAL_ATK_Y + sin(anglea_rad) * CURRENT_DATA_READ.atkGDist; } else { From ae149dc71caee60c9c544dd265af61dbd3a64a9a Mon Sep 17 00:00:00 2001 From: emamaker Date: Thu, 7 Jul 2022 21:52:52 +0200 Subject: [PATCH 11/14] line-sys-camera: better handling of edge cases --- include/systems/lines/linesys_camera.h | 38 +++++++++++---------- src/system/lines/linesys_camera.cpp | 47 +++++++++++++++++++++----- 2 files changed, 59 insertions(+), 26 deletions(-) diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index 9f29b82..eac732e 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -5,7 +5,7 @@ #include "behaviour_control/ds_ctrl.h" #include "systems/systems.h" -#include "vars.h" +#include "vars.h" #define S1I A14 #define S1O A15 @@ -19,23 +19,25 @@ #define LINE_THRESH_CAM 100 #define EXIT_TIME 100 -class LineSysCamera : public LineSystem{ +class LineSysCamera : public LineSystem +{ - public: - LineSysCamera(); - LineSysCamera(vector in_, vector out); +public: + LineSysCamera(); + LineSysCamera(vector in_, vector out); - void update() override; - void test() override; - void outOfBounds(); - void checkLineSensors(); - - private: - vector in, out; - DataSource* ds; - bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; - int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i; - unsigned long exitTimer; - int outDir, outVel; - byte linesens; + void update() override; + void test() override; + void outOfBounds(); + void checkLineSensors(); + bool angleCritic(int angle); + +private: + vector in, out; + DataSource *ds; + bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; + int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i; + unsigned long exitTimer; + int outDir, outVel; + byte linesens; }; \ No newline at end of file diff --git a/src/system/lines/linesys_camera.cpp b/src/system/lines/linesys_camera.cpp index 3922064..e13396b 100644 --- a/src/system/lines/linesys_camera.cpp +++ b/src/system/lines/linesys_camera.cpp @@ -99,19 +99,45 @@ void LineSysCamera::outOfBounds() if (millis() - exitTimer < EXIT_TIME) { + int dir = 0; + int speed = CURRENT_DATA_READ.ySeen || CURRENT_DATA_READ.bSeen ? MAX_VEL : 0; int yangle = CURRENT_DATA_READ.yangle_fix * CURRENT_DATA_READ.ySeen; int bangle = CURRENT_DATA_READ.bangle_fix * CURRENT_DATA_READ.bSeen; + int ydist = CURRENT_DATA_READ.ydist; + int bdist = CURRENT_DATA_READ.bdist; - int diffB = abs(min(yangle, bangle) - max(yangle, bangle)); - int diffB1 = 360 - diffB; - int diff = min(diffB, diffB1); + if (!CURRENT_DATA_READ.bSeen && !CURRENT_DATA_READ.ySeen) + { + dir = 0; + speed = 0; + } + else + { + if (angleCritic(yangle) || angleCritic(bangle)) + { + if(CURRENT_DATA_READ.bSeen && CURRENT_DATA_READ.ySeen){ + dir = ydist > bdist ? yangle : bangle; + }else{ + if (CURRENT_DATA_READ.bSeen) dir = bangle; + else + dir = yangle; + } + } + else + { + int diffB = abs(min(yangle, bangle) - max(yangle, bangle)); + int diffB1 = 360 - diffB; + int diff = min(diffB, diffB1); + dir = min(yangle, bangle) + diff * 0.5f; + } + drive->prepareDrive(dir, speed, CURRENT_DATA_WRITE.tilt); - DEBUG.println("AngleY " + String(yangle) + " AngleB" + String(bangle) + " Dir " + String(min(yangle, bangle) + diff)); + // DEBUG.println("AngleY:" + String(yangle) + " AngleB:" + String(bangle) + " DistY: " + String(ydist) + " DistB: " +String(bdist) + " Dir " + String(dir)); - drive->prepareDrive(min(yangle, bangle) + diff, CURRENT_DATA_READ.ySeen || CURRENT_DATA_READ.bSeen ? MAX_VEL : 0, CURRENT_DATA_WRITE.tilt); - // CURRENT_DATA_WRITE.game->ps->goCenter(); - // tookLine = true; - tone(BUZZER, 220.00, 250); + // CURRENT_DATA_WRITE.game->ps->goCenter(); + // tookLine = true; + tone(BUZZER, 220.00, 250); + } } else { @@ -122,6 +148,11 @@ void LineSysCamera::outOfBounds() } } +bool LineSysCamera::angleCritic(int angle) +{ + return angle >= 355 || angle <= 5 || (angle >= 175 && angle <= 185); +} + void LineSysCamera::test() { update(); From b221a48c41a7dd51083b4d05470bfb9e0cc81a8b Mon Sep 17 00:00:00 2001 From: emamaker Date: Thu, 7 Jul 2022 21:53:11 +0200 Subject: [PATCH 12/14] openmv: recalibration --- utility/OpenMV/conic_angle_dist.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/utility/OpenMV/conic_angle_dist.py b/utility/OpenMV/conic_angle_dist.py index 4bfd115..a6dc1a9 100644 --- a/utility/OpenMV/conic_angle_dist.py +++ b/utility/OpenMV/conic_angle_dist.py @@ -44,7 +44,7 @@ blue_led.on() thresholds = [ (72, 100, -26, 12, 37, 91), # thresholds yellow goalz - (45, 70, -9, 29, -80, -42)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (37, 51, -32, 28, -63, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (40, 0, 260, 240) @@ -57,8 +57,8 @@ sensor.set_contrast(3) sensor.set_saturation(3) sensor.set_brightness(3) sensor.set_auto_whitebal(False, (-6.02073, -5.494869, -0.8559153)) -sensor.set_auto_exposure(False, 5845) -#sensor.set_auto_gain(True) +sensor.set_auto_exposure(False, 8245) +sensor.set_auto_gain(False) sensor.skip_frames(time = 300) clock = time.clock() From f10268aa6dae73a2ef9a152bca6f31be1220a8ca Mon Sep 17 00:00:00 2001 From: emamaker Date: Thu, 7 Jul 2022 21:55:41 +0200 Subject: [PATCH 13/14] pos-sys-cam: position based on distance from goals --- include/systems/position/positionsys_camera.h | 69 +++++++------- src/system/positions/positionsys_camera.cpp | 89 ++++++------------- 2 files changed, 61 insertions(+), 97 deletions(-) diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 3f6fc82..1fa4d6a 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -1,32 +1,31 @@ #pragma once #include "PID_v2.h" -#include "systems/systems.h" #include "behaviour_control/complementary_filter.h" #include "behaviour_control/status_vector.h" +#include "systems/systems.h" -//Camera center: those setpoints correspond to what we consider the center of the field +// Camera center: those setpoints correspond to what we consider the center of the field #define CAMERA_CENTER_X 0 #define CAMERA_CENTER_Y 0 -//left and right limits of a goal +// left and right limits of a goal #define CAMERA_GOAL_MAX_X 8 #define CAMERA_GOAL_MIN_X (-8) -//dimensions of the field, for how we scale it -#define DIM_X 220 -#define DIM_X_HALF 110 -#define DIM_Y 240 -#define DIM_Y_HALF 120 +// dimensions of the field, kinda empirical +#define DIM_X 80 +#define DIM_X_HALF 40 +#define DIM_Y 140 +#define DIM_Y_HALF 70 -//where is the center of a goal blob as seen by openmv on the field. For atk goal it's positive, for def goal it's negative +// where is the center of a goal blob as seen by openmv on the field. For atk goal it's positive, for def goal it's negative #define CAMERA_GOAL_X 0 #define CAMERA_GOAL_Y DIM_Y_HALF #define CAMERA_GOAL_ATK_Y CAMERA_GOAL_Y #define CAMERA_GOAL_DEF_Y (-CAMERA_GOAL_Y) -//hipotenuse of dimensions of field -#define MAX_DIST_EXPERIMENTAL 94 - +// hipotenuse of dimensions of field +#define MAX_DIST_EXPERIMENTAL 94 #define DIST_MULT 8 @@ -40,30 +39,30 @@ #define Kiy 0.1 #define Kdy 0 -class PositionSysCamera : public PositionSystem{ +class PositionSysCamera : public PositionSystem +{ - public: - PositionSysCamera(); - void goCenter() override; - void centerGoal() override; - void setMoveSetpoints(int x, int y); - void addMoveOnAxis(int x, int y); - void update() override; - void test() override; - void setCameraPID(); - void CameraPID(); - bool isInTheVicinityOf(int, int); - bool isInRoughVicinityOf(int, int); - bool isAtDistanceFrom(int, int, int); +public: + PositionSysCamera(); + void goCenter() override; + void centerGoal() override; + void setMoveSetpoints(int x, int y); + void addMoveOnAxis(int x, int y); + void update() override; + void test() override; + void setCameraPID(); + void CameraPID(); + bool isInTheVicinityOf(int, int); + bool isInRoughVicinityOf(int, int); + bool isAtDistanceFrom(int, int, int); - double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; - int MAX_DIST, vx, vy, axisx, axisy, method; - bool givenMovement; - PID* X; - PID* Y; - ComplementaryFilter* filterDir; - ComplementaryFilter* filterSpeed; - - data valid_data; + double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; + int MAX_DIST, vx, vy, axisx, axisy, method; + bool givenMovement; + PID *X; + PID *Y; + ComplementaryFilter *filterDir; + ComplementaryFilter *filterSpeed; + data valid_data; }; diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 4fb8cab..e280e1f 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -44,75 +44,40 @@ void PositionSysCamera::update() { int posx = 0, posy = 0; CURRENT_DATA_WRITE.camera_back_in_time = false; + bool data_valid = true; - double anglea = (double)((90 - CURRENT_DATA_READ.atkGAngle_fix + 360) % 360); - double angled = (double)((270 - CURRENT_DATA_READ.defGAngle_fix + 360) % 360); - - double anglea_rad = radians(anglea); - double angled_rad = radians(angled); - - // DEBUG.println("Angles from goals " + String(anglea) + ", " + String(angled)); - - // Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync - // Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot - if (CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen) + if (CURRENT_DATA_READ.atkSeen || CURRENT_DATA_READ.defSeen) { - // project two lines, from the center of the goals to the robot. The point of intersection of these two lines is the position of the robot - // this doesn't work when the angles have tangents that approach infinity, so filtering for that case is needed + method = 0; + int distxd = -sin(radians(CURRENT_DATA_READ.defGAngle_fix)) * CURRENT_DATA_READ.defGDist; + int distyd = abs(cos(radians(CURRENT_DATA_READ.defGAngle_fix))) * CURRENT_DATA_READ.defGDist; + int distxa = -sin(radians(CURRENT_DATA_READ.atkGAngle_fix)) * CURRENT_DATA_READ.atkGDist; + int distya = abs(cos(radians(CURRENT_DATA_READ.atkGAngle_fix))) * CURRENT_DATA_READ.atkGDist; - if (CURRENT_DATA_READ.atkGAngle_fix >= 355 || CURRENT_DATA_READ.atkGAngle_fix <= 5 || (CURRENT_DATA_READ.defGAngle_fix >= 175 && CURRENT_DATA_READ.defGAngle_fix <= 185)) + // DEBUG.println("POSX1, POSY1 " + String(distxd) + "," + String(distyd)); + // DEBUG.println("POSX2, POSY2 " + String(distxa) + "," + String(distya)); + + int posya = CAMERA_GOAL_ATK_Y - distya; + int posyd = CAMERA_GOAL_DEF_Y + distyd; + + if ((distxd * distxa < 0) || (CURRENT_DATA_READ.atkSeen && CURRENT_DATA_READ.defSeen && posya - posyd > 25)) data_valid = false; + + if (data_valid) { - // fallback to a method without tangents - // Extend two vector and find the point where they end, then take the average - method = 1; - - int posx1 = (int)(cos(angled_rad) * CURRENT_DATA_READ.defGDist); - int posy1 = (int)(CAMERA_GOAL_DEF_Y + sin(angled_rad) * CURRENT_DATA_READ.defGDist); - int posx2 = (int)(cos(anglea_rad) * CURRENT_DATA_READ.atkGDist); - int posy2 = (int)(CAMERA_GOAL_ATK_Y - sin(anglea_rad) * CURRENT_DATA_READ.atkGDist); - - // DEBUG.println("POSX1, POSY1 " + String(posx1) + "," + String(posy1)); - // DEBUG.println("POSX2, POSY2 " + String(posx2) + "," + String(posy2)); - - posx = (int)((posx1 + posx2) * 0.5); - posy = (int)((posy1 + posy2) * 0.5); - } - else - { - // resolved manually and checked with wolfram alpha - // here is the solution https://www.wolframalpha.com/input?i=systems+of+equations+calculator&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation1%22%7D+-%3E%22y-j+%3D+tan%28a%29%28x-i%29%22&assumption=%22FSelect%22+-%3E+%7B%7B%22SolveSystemOf2EquationsCalculator%22%7D%7D&assumption=%7B%22F%22%2C+%22SolveSystemOf2EquationsCalculator%22%2C+%22equation2%22%7D+-%3E%22y-v%3Dtan%28b%29%28x-u%29%22 - //(i,j), (u,v) are the coords of the two goals. Some stuff cancels out since we assume that the goals always have 0 as x coord - method = 0; - - double tana = tan(anglea_rad); - double tanb = tan(angled_rad); - - double tana_tanb_diff = tana - tanb; - - double posx_n = CAMERA_GOAL_DEF_Y - CAMERA_GOAL_ATK_Y; - double posy_n = -CAMERA_GOAL_ATK_Y * tanb + CAMERA_GOAL_DEF_Y * tana; - - posx = (int)(posx_n / tana_tanb_diff); - posy = (int)(posy_n / tana_tanb_diff); + if ((CURRENT_DATA_READ.atkSeen && !CURRENT_DATA_READ.defSeen) || (CURRENT_DATA_READ.atkSeen && CURRENT_DATA_READ.defSeen && CURRENT_DATA_READ.atkGDist < CURRENT_DATA_READ.defGDist)) + { + posx = distxa; + posy = posya; + } + else + { + posx = distxd; + posy = posyd; + } } } - else if (!CURRENT_DATA_WRITE.atkSeen && CURRENT_DATA_WRITE.defSeen) - { - method = 2; - // Extend a vector from a known point and reach the position of the robot - posx = CAMERA_GOAL_X + cos(angled_rad) * CURRENT_DATA_READ.defGDist; - posy = CAMERA_GOAL_DEF_Y + sin(angled_rad) * CURRENT_DATA_READ.defGDist; - } - else if (CURRENT_DATA_WRITE.atkSeen && !CURRENT_DATA_WRITE.defSeen) - { - method = 3; - - // Extend a vector from a known point and reach the position of the robot - posx = CAMERA_GOAL_X + cos(anglea_rad) * CURRENT_DATA_READ.atkGDist; - posy = CAMERA_GOAL_ATK_Y + sin(anglea_rad) * CURRENT_DATA_READ.atkGDist; - } - else + if (!data_valid || (!CURRENT_DATA_READ.atkSeen && !CURRENT_DATA_READ.defSeen)) { method = 4; From 1508853ebb2fb3ce133255d1e96dae8cd65ff697 Mon Sep 17 00:00:00 2001 From: emamaker Date: Thu, 7 Jul 2022 22:02:20 +0200 Subject: [PATCH 14/14] camera: filter fixed angles too --- src/sensors/data_source_camera_conicmirror.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index dd1d320..db037cf 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -100,6 +100,13 @@ void DataSourceCameraConic ::computeCoordsAngles() yangle_fix = (yangle + tmp + 360) % 360; bangle_fix = (bangle + tmp + 360) % 360; +#ifdef CAMERA_CONIC_FILTER_POINTS + if (CURRENT_DATA_WRITE.ySeen) + yangle_fix = filt_yangle_fix->calculate(yangle_fix); + if (CURRENT_DATA_WRITE.bSeen) + bangle_fix = filt_bangle_fix->calculate(bangle_fix); +#endif + // TODO: Maybe add a complementary filter on fixed angles ? // Important: update status vector