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 5de0222..49a30f1 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 -12 +#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 +#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/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/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 755c657..1fa4d6a 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -1,26 +1,32 @@ #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 -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, 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 +#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 @@ -33,31 +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(); - int calcOtherGoalY(int goalY); - 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; - 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/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index b2d54bc..db037cf 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -1,246 +1,136 @@ -#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()) + { + 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; + s1 = ""; + s2 = ""; + } + // 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(); + } + computeCoordsAngles(); + } + + end = true; + start = false; + dash = false; + count = 0; + } // dash + else if (current_char == '-' && start) + { + dash = true; + } // it's a number + else if (isDigit(current_char) && start) + { + 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; + CURRENT_DATA_WRITE.bSeen = bangle != 999; + CURRENT_DATA_WRITE.ySeen = yangle != 999; - //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; +#ifdef CAMERA_CONIC_FILTER_POINTS + 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 - #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 + // Fix angles using the IMU + int tmp = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; - //-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; + yangle_fix = (yangle + tmp + 360) % 360; + bangle_fix = (bangle + tmp + 360) % 360; - // //Fixes with IMU - // yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360; - // bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 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 - //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))); + // TODO: Maybe add a complementary filter on fixed angles ? - #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 + // 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; - yAngleFix = -90 + (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14); - bAngleFix = -90 + (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14); + 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; - //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("---------------"); - delay(150); +void DataSourceCameraConic::test() +{ + 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); } 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..e13396b 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,9 +99,45 @@ void LineSysCamera::outOfBounds() if (millis() - exitTimer < EXIT_TIME) { - CURRENT_DATA_WRITE.game->ps->goCenter(); - tookLine = true; - tone(BUZZER, 220.00, 250); + 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; + + 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) + " DistY: " + String(ydist) + " DistB: " +String(bdist) + " Dir " + String(dir)); + + // CURRENT_DATA_WRITE.game->ps->goCenter(); + // tookLine = true; + tone(BUZZER, 220.00, 250); + } } else { @@ -112,6 +148,11 @@ void LineSysCamera::outOfBounds() } } +bool LineSysCamera::angleCritic(int angle) +{ + return angle >= 355 || angle <= 5 || (angle >= 175 && angle <= 185); +} + void LineSysCamera::test() { update(); diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 83580ce..e280e1f 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -1,20 +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() { - MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y); - +PositionSysCamera::PositionSysCamera() +{ Inputx = 0; Outputx = 0; Setpointx = 0; @@ -27,13 +26,13 @@ PositionSysCamera::PositionSysCamera() { axisx = 0; axisy = 0; 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); @@ -41,54 +40,73 @@ 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; + bool data_valid = true; + + if (CURRENT_DATA_READ.atkSeen || CURRENT_DATA_READ.defSeen) + { + 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; + + // 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) + { + 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; + } + } + } + + if (!data_valid || (!CURRENT_DATA_READ.atkSeen && !CURRENT_DATA_READ.defSeen)) + { + method = 4; - //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 - - 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; // 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; } } - } 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; @@ -96,8 +114,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; @@ -106,18 +125,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); } @@ -126,29 +148,29 @@ 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_){ +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; @@ -156,42 +178,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_READ.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(){ +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..a6dc1a9 --- /dev/null +++ b/utility/OpenMV/conic_angle_dist.py @@ -0,0 +1,157 @@ +# 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 + (37, 51, -32, 28, -63, -25)] # 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, 8245) +sensor.set_auto_gain(False) +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 = math.atan2(y_cy, y_cx) + y_angle = -90 + (y_angle * 180 / 3.14) #convert to degrees and shift + 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 = int(math.sqrt(y_cx*y_cx + y_cy*y_cy)) + + ydata = "{}{}{}{}{}".format("Y", str(y_angle), "-", str(y_dist), "y") + # il numero 999 indica che non ho trovato nessun blob + else: + ydata = "{}{}{}{}{}".format("Y", "999", "-", "0", "y") + + + # trasmetto dati in seriale e test su terminale + uart.write(ydata) + + + 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 = 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 = (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 = int(math.sqrt(b_cx*b_cx + b_cy*b_cy)) + + bdata = "{}{}{}{}{}".format("B", str(b_angle), "-", str(b_dist), "b") + # il numero 999 indica che non ho trovato nessun blob + else: + bdata = "{}{}{}{}{}".format("B", "999", "-", "0", "b") + + uart.write(bdata) + + + #BLUE FIRST, YELLOW SECOND. ANGLE FIRST, DISTANCE SECOND + #print(str(b_angle) + " | " + str(b_dist) + " --- " + str(y_angle) + " | " + str(y_dist)) + print(f"Blue {bdata}") + print(f"Yellow {ydata}")