Merge pull request 'Switch to using distance and angles from goals' (#2) from openmv into master
Reviewed-on: #2master
EmaMaker
commit
8fa363e44e
|
|
@ -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 -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;
|
||||
};
|
||||
|
|
@ -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<DataSource*> in_, vector<DataSource*> out);
|
||||
public:
|
||||
LineSysCamera();
|
||||
LineSysCamera(vector<DataSource *> in_, vector<DataSource *> out);
|
||||
|
||||
void update() override;
|
||||
void test() override;
|
||||
void outOfBounds();
|
||||
void checkLineSensors();
|
||||
|
||||
private:
|
||||
vector<DataSource*> 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<DataSource *> 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;
|
||||
};
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,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();
|
||||
|
|
|
|||
|
|
@ -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) + ")");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,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}")
|
||||
Loading…
Reference in New Issue