EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request 'Switch to using distance and angles from goals' (#2) from openmv into master 

Reviewed-on: #2
master
EmaMaker 2022-07-07 22:29:54 +02:00
parent 74a053ff9e cee5c39599
commit 8fa363e44e
11 changed files with 524 additions and 431 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
include/behaviour_control/status_vector.h
View File

@ -30,30 +30,26 @@
typedef struct input{ typedef struct input{
int IMUAngle, USfr, USsx, USdx, USrr, BT; 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; bool SW_DX, SW_SX;
}input; }input;
typedef struct data{ typedef struct data{
int IMUAngle = 0, IMUOffset = 0, ballAngle = 0, ballAngleFix, ballDistance, ballPresenceVal, 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], speed, tilt, dir, axisBlock[4],
USfr, USsx, USdx, USrr, USfr, USsx, USdx, USrr,
lineOutDir, matePos, role, cam_xb_fixed = 0, lineOutDir, matePos, role;
cam_xy_fixed = 0, cam_yb_fixed = 0, cam_yy_fixed = 0, int yangle, bangle, ydist, bdist, yangle_fix, bangle_fix, atkGAngle, defGAngle, atkGAngle_fix, defGAngle_fix, atkGDist, defGDist;
posx, posy; int posx, posy;
float addvx, addvy; float addvx, addvy;
Game* game; Game* game;
LineSystem* lineSystem; LineSystem* lineSystem;
PositionSystem* posSystem; PositionSystem* posSystem;
byte lineSeen, lineActive; byte lineSeen, lineActive;
bool mate, bool mate,
ATKgoal, DEFgoal, atkSeen = false, defSeen = false, bSeen = false, ySeen = false, ballSeen, ballPresent;
atkSeen, defSeen, bSeen = true, ySeen = false, bool camera_back_in_time = false; //whether or not we copied camera position from and older status
ballSeen, ballPresent;
}data; }data;
sv_extr input inputs[dim]; sv_extr input inputs[dim];

43
include/sensors/data_source_camera_conicmirror.h
View File

@ -1,29 +1,17 @@
#pragma once #pragma once
#include <Arduino.h>
#include "behaviour_control/data_source.h" #include "behaviour_control/data_source.h"
#include "behaviour_control/complementary_filter.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_DEFAULT_COEFF 0.85
#define FILTER_BY_COEFF FILTER_DEFAULT_COEFF #define FILTER_YANGLE_COEFF FILTER_DEFAULT_COEFF
#define FILTER_BX_COEFF FILTER_DEFAULT_COEFF #define FILTER_BANGLE_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YY_COEFF FILTER_DEFAULT_COEFF #define FILTER_YANGLE_FIX_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YX_COEFF FILTER_DEFAULT_COEFF #define FILTER_BANGLE_FIX_COEFF FILTER_DEFAULT_COEFF
#define FILTER_YDIST_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 #define FILTER_BDIST_COEFF FILTER_DEFAULT_COEFF
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
class DataSourceCameraConic : public DataSource{ class DataSourceCameraConic : public DataSource{
@ -35,15 +23,14 @@ class DataSourceCameraConic : public DataSource{
// int getValueAtk(bool); // int getValueAtk(bool);
// int getValueDef(bool); // int getValueDef(bool);
int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist; int count = 0, unkn_counter = 0;
bool data_received = false, start = false, end = false, dash = false;
int count = 0, unkn_counter; char current_char = 'a', start_char = 'a', end_char = 'a'; //initialize to unused values
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 goalOrientation, old_goalOrientation, pAtk, pDef; 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;
}; };

4
include/systems/lines/linesys_camera.h
View File

@ -19,7 +19,8 @@
#define LINE_THRESH_CAM 100 #define LINE_THRESH_CAM 100
#define EXIT_TIME 100 #define EXIT_TIME 100
class LineSysCamera : public LineSystem{ class LineSysCamera : public LineSystem
{
public: public:
LineSysCamera(); LineSysCamera();
@ -29,6 +30,7 @@ class LineSysCamera : public LineSystem{
void test() override; void test() override;
void outOfBounds(); void outOfBounds();
void checkLineSensors(); void checkLineSensors();
bool angleCritic(int angle);
private: private:
vector<DataSource *> in, out; vector<DataSource *> in, out;

39
include/systems/position/positionsys_camera.h
View File

@ -1,26 +1,32 @@
#pragma once #pragma once
#include "PID_v2.h" #include "PID_v2.h"
#include "systems/systems.h"
#include "behaviour_control/complementary_filter.h" #include "behaviour_control/complementary_filter.h"
#include "behaviour_control/status_vector.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_X 0
#define CAMERA_CENTER_Y -20 #define CAMERA_CENTER_Y 0
// left and right limits of a goal
//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_GOAL_MAX_X 8 #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 DIST_MULT 8
#define VICINITY_DIST_TRESH 2 #define VICINITY_DIST_TRESH 2
@ -33,7 +39,8 @@
#define Kiy 0.1 #define Kiy 0.1
#define Kdy 0 #define Kdy 0
class PositionSysCamera : public PositionSystem{ class PositionSysCamera : public PositionSystem
{
public: public:
PositionSysCamera(); PositionSysCamera();
@ -45,13 +52,12 @@ class PositionSysCamera : public PositionSystem{
void test() override; void test() override;
void setCameraPID(); void setCameraPID();
void CameraPID(); void CameraPID();
int calcOtherGoalY(int goalY);
bool isInTheVicinityOf(int, int); bool isInTheVicinityOf(int, int);
bool isInRoughVicinityOf(int, int); bool isInRoughVicinityOf(int, int);
bool isAtDistanceFrom(int, int, int); bool isAtDistanceFrom(int, int, int);
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
int MAX_DIST, vx, vy, axisx, axisy; int MAX_DIST, vx, vy, axisx, axisy, method;
bool givenMovement; bool givenMovement;
PID *X; PID *X;
PID *Y; PID *Y;
@ -59,5 +65,4 @@ class PositionSysCamera : public PositionSystem{
ComplementaryFilter *filterSpeed; ComplementaryFilter *filterSpeed;
data valid_data; data valid_data;
}; };

308
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -1,246 +1,136 @@
#include "behaviour_control/status_vector.h"
#include "sensors/data_source_camera_conicmirror.h" #include "sensors/data_source_camera_conicmirror.h"
#include "behaviour_control/status_vector.h"
#include "vars.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 #define CAMERA_CONIC_FILTER_POINTS
DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) { DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud)
true_xb = 0; {
true_yb = 0; filt_yangle = new ComplementaryFilter(FILTER_YANGLE_COEFF);
true_xy = 0; filt_bangle = new ComplementaryFilter(FILTER_BANGLE_COEFF);
true_yy = 0; filt_ydist = new ComplementaryFilter(FILTER_YDIST_COEFF);
true_xb_fixed = 0; filt_bdist = new ComplementaryFilter(FILTER_BDIST_COEFF);
true_yb_fixed = 0; filt_yangle_fix = new ComplementaryFilter(FILTER_YANGLE_FIX_COEFF);
true_xy_fixed = 0; filt_bangle_fix = new ComplementaryFilter(FILTER_BANGLE_FIX_COEFF);
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);
} }
void DataSourceCameraConic ::readSensor() { void DataSourceCameraConic ::readSensor()
while (ser->available() > 0) { {
value = (int)ser->read(); while (ser->available())
//Serial.println(value); {
if (value == startp) { current_char = ser->read();
// update status vector
CURRENT_INPUT_WRITE.cameraChar = current_char;
// start
if (current_char == 'B' || current_char == 'Y')
{
start_char = current_char;
start = true; start = true;
count = 0; end = false;
} else if (value == endp) { dash = false;
data_received = false; s1 = "";
if ((count == 4) && (start == true)) { s2 = "";
data_received = true; }
// end
else if (current_char == 'b' || current_char == 'y')
{
end_char = current_char;
true_xb = xb; if (start && dash && ((start_char == 'B' && end_char == 'b') || (start_char == 'Y' && end_char == 'y')))
true_yb = yb; {
true_xy = xy;
true_yy = yy;
if (start_char == 'B')
{
bangle = s1.toInt();
bdist = s2.toInt();
}
else
{
yangle = s1.toInt();
ydist = s2.toInt();
}
computeCoordsAngles(); computeCoordsAngles();
} }
end = true; end = true;
start = false; start = false;
} else{ dash = false;
if (start == true) count = 0;
} // dash
else if (current_char == '-' && start)
{ {
if (count == 0) dash = true;
xb = value; } // it's a number
else if (count == 1) else if (isDigit(current_char) && start)
yb = value; {
else if (count == 2) if (dash)
xy = value; s2 += current_char;
else if (count == 3) else
yy = value; s1 += current_char;
count++;
}
} }
} }
} }
// Angles are received in degrees
void DataSourceCameraConic ::computeCoordsAngles()
{
void DataSourceCameraConic ::computeCoordsAngles() { CURRENT_DATA_WRITE.bSeen = bangle != 999;
CURRENT_DATA_WRITE.bSeen = true_xb != unkn && true_yb != unkn; CURRENT_DATA_WRITE.ySeen = yangle != 999;
CURRENT_DATA_WRITE.ySeen = true_xy != unkn && true_yy != unkn;
//Where are the goals relative to the robot?
//Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them, getting the original coords calculated by the camera
true_xb = 50 - true_xb + CAMERA_TRANSLATION_X*0.5;
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 #ifdef CAMERA_CONIC_FILTER_POINTS
true_xb = filter_xb->calculate(true_xb); if (CURRENT_DATA_WRITE.ySeen)
true_yb = filter_yb->calculate(true_yb); {
true_xy = filter_xy->calculate(true_xy); yangle = filt_yangle->calculate(yangle);
true_yy = filter_yy->calculate(true_yy); ydist = filt_ydist->calculate(ydist);
}
if (CURRENT_DATA_WRITE.bSeen)
{
bangle = filt_bangle->calculate(bangle);
bdist = filt_bdist->calculate(bdist);
}
#endif #endif
//-90 + to account for phase shifting with goniometric circle // Fix angles using the IMU
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;
// //Fixes with IMU
// yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360;
// bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360;
//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; 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 yangle_fix = (yangle + tmp + 360) % 360;
//A little explanation of the formula used here can be found on wikipedia: https://en.wikipedia.org/wiki/Rotation_of_axes bangle_fix = (bangle + tmp + 360) % 360;
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)));
#ifdef CAMERA_CONIC_FILTER_POINTS #ifdef CAMERA_CONIC_FILTER_POINTS
true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed); if (CURRENT_DATA_WRITE.ySeen)
true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed); yangle_fix = filt_yangle_fix->calculate(yangle_fix);
true_xy_fixed = filter_xy_fix->calculate(true_xy_fixed); if (CURRENT_DATA_WRITE.bSeen)
true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed); bangle_fix = filt_bangle_fix->calculate(bangle_fix);
#endif #endif
yAngleFix = -90 + (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14); // TODO: Maybe add a complementary filter on fixed angles ?
bAngleFix = -90 + (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
// Important: update status vector // Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value; CURRENT_DATA_WRITE.yangle = yangle;
CURRENT_DATA_WRITE.cam_xb = true_xb; CURRENT_DATA_WRITE.bangle = bangle;
CURRENT_DATA_WRITE.cam_yb = true_yb; CURRENT_DATA_WRITE.yangle_fix = yangle_fix;
CURRENT_DATA_WRITE.cam_xy = true_xy; CURRENT_DATA_WRITE.bangle_fix = bangle_fix;
CURRENT_DATA_WRITE.cam_yy = true_yy; CURRENT_DATA_WRITE.ydist = ydist;
CURRENT_DATA_WRITE.cam_xb_fixed = true_xb_fixed; CURRENT_DATA_WRITE.bdist = bdist;
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.atkGAngle = goalOrientation ? yangle : bangle;
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle; CURRENT_DATA_WRITE.atkGAngle_fix = goalOrientation ? yangle_fix : bangle_fix;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix; CURRENT_DATA_WRITE.atkGDist = goalOrientation ? ydist : bdist;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen; CURRENT_DATA_WRITE.atkSeen = goalOrientation ? CURRENT_DATA_WRITE.ySeen : CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.distAtk = CURRENT_DATA_WRITE.yDist;
CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yy; CURRENT_DATA_WRITE.defGAngle = !goalOrientation ? yangle : bangle;
CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed; CURRENT_DATA_WRITE.defGAngle_fix = !goalOrientation ? yangle_fix : bangle_fix;
CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xy; CURRENT_DATA_WRITE.defGDist = !goalOrientation ? ydist : bdist;
CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xy_fixed; CURRENT_DATA_WRITE.defSeen = !goalOrientation ? CURRENT_DATA_WRITE.ySeen : CURRENT_DATA_WRITE.bSeen;
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; void DataSourceCameraConic::test()
// to_32u4 |= (CURRENT_DATA_READ.ySeen); {
// to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1; DEBUG.println("Received char '" + String(CURRENT_INPUT_READ.cameraChar) + "'");
// BALL_32U4.write(to_32u4); 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);
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);
} }

12
src/strategy_roles/striker.cpp
View File

@ -39,9 +39,9 @@ void Striker::striker()
if (CURRENT_DATA_READ.lineActive != 0) flag = false; 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); 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()) if (ball->isInFront())
{ {
plusang = STRIKER_PLUSANG_VISIONCONE;
// dir = ball_deg >= 0 ? plusang : - plusang;
dir = 0; dir = 0;
flag = false; flag = false;
} }
@ -78,9 +76,8 @@ void Striker::striker()
dir = ball_deg + plusang_flag; dir = ball_deg + plusang_flag;
} }
if (CURRENT_DATA_READ.atkSeen) atk_tilt = CURRENT_DATA_READ.angleAtkFix;
dir = (dir + 360) % 360; 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); // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
// else roller->speed(roller->MIN); // else roller->speed(roller->MIN);
@ -89,6 +86,5 @@ void Striker::striker()
int Striker::tilt() int Striker::tilt()
{ {
// return CURRENT_DATA_READ.ballAngle <= 110 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0; return CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.atkGAngle_fix : 0;
return CURRENT_DATA_READ.angleAtkFix;
} }

45
src/system/lines/linesys_camera.cpp
View File

@ -99,10 +99,46 @@ void LineSysCamera::outOfBounds()
if (millis() - exitTimer < EXIT_TIME) if (millis() - exitTimer < EXIT_TIME)
{ {
CURRENT_DATA_WRITE.game->ps->goCenter(); int dir = 0;
tookLine = true; 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); tone(BUZZER, 220.00, 250);
} }
}
else else
{ {
// drive->canUnlock = true; // drive->canUnlock = true;
@ -112,6 +148,11 @@ void LineSysCamera::outOfBounds()
} }
} }
bool LineSysCamera::angleCritic(int angle)
{
return angle >= 355 || angle <= 5 || (angle >= 175 && angle <= 185);
}
void LineSysCamera::test() void LineSysCamera::test()
{ {
update(); update();

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

@ -1,20 +1,19 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h" #include "systems/position/positionsys_camera.h"
#include "behaviour_control/status_vector.h"
#include "math.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "vars.h" #include "vars.h"
#include "math.h"
int diff(int a, int b)
int diff(int a, int b){ {
int diffB = abs(min(a, b) - max(a, b)); int diffB = abs(min(a, b) - max(a, b));
int diffB1 = 360 - diffB; int diffB1 = 360 - diffB;
int diff = min(diffB, diffB1); int diff = min(diffB, diffB1);
return diff; return diff;
} }
PositionSysCamera::PositionSysCamera() { PositionSysCamera::PositionSysCamera()
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y); {
Inputx = 0; Inputx = 0;
Outputx = 0; Outputx = 0;
Setpointx = 0; Setpointx = 0;
@ -29,11 +28,11 @@ PositionSysCamera::PositionSysCamera() {
givenMovement = false; givenMovement = false;
X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); 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->SetMode(AUTOMATIC);
X->SetSampleTime(2); X->SetSampleTime(2);
Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); 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->SetMode(AUTOMATIC);
Y->SetSampleTime(2); Y->SetSampleTime(2);
@ -41,52 +40,71 @@ PositionSysCamera::PositionSysCamera() {
filterSpeed = new ComplementaryFilter(0.65); filterSpeed = new ComplementaryFilter(0.65);
} }
void PositionSysCamera::update(){ void PositionSysCamera::update()
{
int posx = 0, posy = 0; 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 // 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); valid_data = getDataAtIndex_backwardsFromCurrent(i);
if(valid_data.ySeen || valid_data.bSeen){ if (valid_data.ySeen || valid_data.bSeen)
{
posx = valid_data.posx; posx = valid_data.posx;
posy = valid_data.posy; posy = valid_data.posy;
// Trick the status vector into thinking this was a valid status // Trick the status vector into thinking this was a valid status
CURRENT_DATA_WRITE.ySeen = valid_data.ySeen; CURRENT_DATA_WRITE.ySeen = valid_data.ySeen;
CURRENT_DATA_WRITE.bSeen = valid_data.bSeen; CURRENT_DATA_WRITE.bSeen = valid_data.bSeen;
CURRENT_DATA_WRITE.camera_back_in_time = true;
break; break;
} }
} }
} }
CURRENT_DATA_WRITE.posx = posx; CURRENT_DATA_WRITE.posx = posx;
CURRENT_DATA_WRITE.posy = posy; CURRENT_DATA_WRITE.posy = posy;
Inputx = posx; Inputx = posx;
Inputy = posy; Inputy = posy;
// Prepare for receiving information about movement // Prepare for receiving information about movement
// Starting setpoint position as current position // Starting setpoint position as current position
Setpointx = posx; Setpointx = posx;
@ -97,7 +115,8 @@ void PositionSysCamera::update(){
} }
// This means the last time this is called has the biggest priority, has for prepareDrive // This means the last time this is called has the biggest priority, has for prepareDrive
void PositionSysCamera::setMoveSetpoints(int x, int y){ void PositionSysCamera::setMoveSetpoints(int x, int y)
{
// Setpointx = x + CAMERA_TRANSLATION_X; // Setpointx = x + CAMERA_TRANSLATION_X;
// Setpointy = y + CAMERA_TRANSLATION_Y; // Setpointy = y + CAMERA_TRANSLATION_Y;
Setpointx = x; Setpointx = x;
@ -106,18 +125,21 @@ void PositionSysCamera::setMoveSetpoints(int x, int y){
CameraPID(); CameraPID();
} }
void PositionSysCamera::addMoveOnAxis(int x, int y){ void PositionSysCamera::addMoveOnAxis(int x, int y)
{
axisx += x; axisx += x;
axisy += y; axisy += y;
givenMovement = true; givenMovement = true;
CameraPID(); CameraPID();
} }
void PositionSysCamera::goCenter(){ void PositionSysCamera::goCenter()
{
setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y);
} }
void PositionSysCamera::centerGoal(){ void PositionSysCamera::centerGoal()
{
setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y); 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 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 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 // 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 // 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 // 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(){ void PositionSysCamera::CameraPID()
if(givenMovement){ {
if (givenMovement)
{
vx = 0; vx = 0;
vy = 0; vy = 0;
@ -156,7 +178,8 @@ void PositionSysCamera::CameraPID(){
Setpointx += axisx; Setpointx += axisx;
Setpointy += axisy; Setpointy += axisy;
if(X->Compute() && Y->Compute()){ if (X->Compute() && Y->Compute())
{
// Compute an X and Y to give to the PID later // Compute an X and Y to give to the PID later
// There's surely a better way to do this // There's surely a better way to do this
@ -181,17 +204,16 @@ void PositionSysCamera::CameraPID(){
CURRENT_DATA_WRITE.addvx = vx; CURRENT_DATA_WRITE.addvx = vx;
CURRENT_DATA_WRITE.addvy = vy; CURRENT_DATA_WRITE.addvy = vy;
#else #else
int tmp = (CURRENT_DATA_READ.tilt+360)%360; int tmp = (CURRENT_DATA_WRITE.tilt + 360) % 360;
dir = dir - tmp; dir = dir - tmp;
if (dir < 0) dir += 360; if (dir < 0) dir += 360;
drive->prepareDrive(dir, speed, CURRENT_DATA_WRITE.tilt); drive->prepareDrive(dir, speed, CURRENT_DATA_WRITE.tilt);
#endif #endif
} }
} }
} }
void PositionSysCamera::test()
void PositionSysCamera::test(){ {
DEBUG.println("Using method " + String(method) + " Position: (" + String(CURRENT_DATA_WRITE.posx) + ", " + String(CURRENT_DATA_WRITE.posy) + ")");
} }

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

@ -263,9 +263,9 @@ void PositionSysZone::phyZoneUS(){
void PositionSysZone::phyZoneCam(){ void PositionSysZone::phyZoneCam(){
//IMU-fixed attack angle //IMU-fixed attack angle
camA = CURRENT_DATA_READ.angleAtkFix; camA = CURRENT_DATA_READ.atkGAngle_fix;
//IMU-fixed defence angle //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 //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 //Positive angle means that the robot is positioned to the left of the goalpost

9
src/test_menu.cpp
View File

@ -83,7 +83,7 @@ void TestMenu::testMenu()
case '8': case '8':
DEBUG.println("Camera tilt Test"); DEBUG.println("Camera tilt Test");
drive->resetDrive(); drive->resetDrive();
drive->prepareDrive(0, 0, CURRENT_DATA_READ.angleAtkFix); drive->prepareDrive(0, 0, CURRENT_DATA_READ.atkGAngle_fix);
break; break;
case '9': case '9':
DEBUG.println("Switches as seen from behind:"); DEBUG.println("Switches as seen from behind:");
@ -96,11 +96,8 @@ void TestMenu::testMenu()
case 'p': case 'p':
if(millis() - timer > 150){ if(millis() - timer > 150){
timer = millis(); timer = millis();
DEBUG.println("Position on the field (camera)"); DEBUG.println("Position on the field");
DEBUG.print("X: "); CURRENT_DATA_WRITE.game->ps->test();
DEBUG.print(CURRENT_DATA_READ.posx);
DEBUG.print(" , Y: ");
DEBUG.println(CURRENT_DATA_READ.posy);
} }
break; break;
case 'u': case 'u':

157
utility/OpenMV/conic_angle_dist.py Normal file
View File

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