camera: switch to using angles and dist
EmaMaker
emamaker
parent
701789b823
commit
5deb6a9af1
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -1,29 +1,17 @@
|
|||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#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;
|
||||
};
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -31,7 +31,7 @@ LineSysCamera::LineSysCamera(vector<DataSource *> in_, vector<DataSource *> 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
|
||||
|
|
|
|||
|
|
@ -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) + ")" );
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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':
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
Loading…
Reference in New Issue