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

aaaaaaaaaaaaaaaaA

pull/1/head
EmaMaker 2020-02-26 16:00:26 +01:00
parent 42d73d7e85
commit e0c7c569f6
7 changed files with 125 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/positionsys_camera.h
View File

@ -1,7 +1,7 @@
#include "systems.h" #include "systems.h"
#define CAMERA_CENTER_X 0 #define CAMERA_CENTER_X 3
#define CAMERA_CENTER_Y 0 #define CAMERA_CENTER_Y 6
class PositionSysCamera : public PositionSystem{ class PositionSysCamera : public PositionSystem{

3
include/status_vector.h
View File

@ -39,13 +39,14 @@ typedef struct data{
yAngle, bAngle, yAngleFix, bAngleFix, yAngle, bAngle, yAngleFix, bAngleFix,
yDist, bDist, yDist, bDist,
angleAtk, angleAtkFix, angleDef, angleDefFix, angleAtk, angleAtkFix, angleDef, angleDefFix,
cam_xb, cam_yb, cam_xy, cam_yy,
speed, tilt, dir, axisBlock[4], speed, tilt, dir, axisBlock[4],
USfr, USsx, USdx, USrr, USfr, USsx, USdx, USrr,
lineOutDir, matePos, role; lineOutDir, matePos, role;
Game* game; Game* game;
LineSystem* lineSystem; LineSystem* lineSystem;
PositionSystem* posSystem; PositionSystem* posSystem;
byte xb, yb, xy, yy, lineSeen, lineActive; byte lineSeen, lineActive;
bool mate, bool mate,
ATKgoal, DEFgoal, ATKgoal, DEFgoal,
atkSeen, defSeen, bSeen, ySeen, atkSeen, defSeen, bSeen, ySeen,

194
src/data_source_camera_conicmirror.cpp
View File

@ -1,7 +1,8 @@
#include "data_source_camera_conicmirror.h" #include "data_source_camera_conicmirror.h"
#include "status_vector.h" #include "status_vector.h"
DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud)
{
true_xb = 0; true_xb = 0;
true_yb = 0; true_yb = 0;
true_xy = 0; true_xy = 0;
@ -21,88 +22,98 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : D
bDist = 0; bDist = 0;
} }
void DataSourceCameraConic :: readSensor(){ void DataSourceCameraConic ::readSensor() {
while(ser->available() > 0){ while (ser->available() > 0) {
value = (int)ser->read(); value = (int)ser->read();
//Serial.println(value); //Serial.println(value);
if(value==startp){ if (value == startp) {
start=true; start = true;
count=0; count = 0;
} } else if (value == endp) {
else if(value==endp){ data_received = false;
data_received=false; if (count = 4 && start == true) {
if(count=4 && start==true) { data_received = true;
data_received=true;
true_xb = xb-50; true_xb = xb - 50;
true_yb = 50-yb; true_yb = 50 - yb;
true_xy = xy-50; true_xy = xy - 50;
true_yy = 50-yy; true_yy = 50 - yy;
//Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them //Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them
yAngle = -90-(atan2(true_yy, true_xy) * 180 /3.14); yAngle = -90 - (atan2(true_yy, true_xy) * 180 / 3.14);
bAngle = -90-(atan2(true_yb, true_xb)* 180 /3.14); bAngle = -90 - (atan2(true_yb, true_xb) * 180 / 3.14);
//Now cast angles to [0, 359] domain angle flip them //Now cast angles to [0, 359] domain angle flip them
yAngle = (yAngle+360)%360; yAngle = (yAngle + 360) % 360;
bAngle = (bAngle+360)%360; bAngle = (bAngle + 360) % 360;
int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
//Fixes with IMU //Fixes with IMU
yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ; yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360;
bAngleFix = ((int) ((bAngle + angleFix*0.8)) + 360) % 360; bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360;
yDist = sqrt( (true_yy-50)*(true_yy-50) + (50-true_xy)*(50-true_xy) ); yDist = sqrt((true_yy - 50) * (true_yy - 50) + (50 - true_xy) * (50 - true_xy));
bDist = sqrt( (true_yb-50)*(true_yb-50) + (50-true_xb)*(50-true_xb) ); bDist = sqrt((true_yb - 50) * (true_yb - 50) + (50 - true_xb) * (50 - true_xb));
//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.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 (xb == unkn || yb == unkn)
CURRENT_DATA_WRITE.bSeen = false;
else
CURRENT_DATA_WRITE.bSeen = true;
if (xy == unkn || yy == unkn)
CURRENT_DATA_WRITE.ySeen = false;
else
CURRENT_DATA_WRITE.ySeen = true;
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.angleDef = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
} else {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
}
} }
end=true; end = true;
start=false; start = false;
}else{ }
if(start==true){ else
if (count==0) xb=value; {
else if (count==1) yb=value; if (start == true)
else if (count==2) xy=value; {
else if (count==3) yy=value; if (count == 0)
xb = value;
else if (count == 1)
yb = value;
else if (count == 2)
xy = value;
else if (count == 3)
yy = value;
count++; count++;
} }
}
//Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value;
CURRENT_DATA_WRITE.xb = true_xb;
CURRENT_DATA_WRITE.yb = true_yb;
CURRENT_DATA_WRITE.xy = true_xy;
CURRENT_DATA_WRITE.yy = true_yy;
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(xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false;
else CURRENT_DATA_WRITE.bSeen = true;
if(xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false;
else CURRENT_DATA_WRITE.ySeen = true;
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.angleDef = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
}else{
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
} }
} }
} }
// int DataSource<CameraConic::getValueAtk(bool fixed){ // int DataSource<CameraConic::getValueAtk(bool fixed){
// if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix; // if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
// else return goalOrientation == HIGH ? yAngle : bAngle; // else return goalOrientation == HIGH ? yAngle : bAngle;
@ -112,32 +123,33 @@ void DataSourceCameraConic :: readSensor(){
// else return goalOrientation == LOW ? yAngle : bAngle; // else return goalOrientation == LOW ? yAngle : bAngle;
// }> // }>
void DataSourceCameraConic::test(){ void DataSourceCameraConic::test()
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu {
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
update(); update();
DEBUG.print("Blue: "); DEBUG.print("Blue: ");
DEBUG.print(bAngle); DEBUG.print(bAngle);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.print(bAngleFix); DEBUG.print(bAngleFix);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.println(bDist); DEBUG.println(bDist);
DEBUG.println(" --- "); DEBUG.println(" --- ");
DEBUG.print("Yellow: "); DEBUG.print("Yellow: ");
DEBUG.print(yAngle); DEBUG.print(yAngle);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.print(yAngleFix); DEBUG.print(yAngleFix);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.println(yDist); DEBUG.println(yDist);
DEBUG.println("---------------"); DEBUG.println("---------------");
DEBUG.print("Data: "); DEBUG.print("Data: ");
DEBUG.print(true_xb); DEBUG.print(true_xb);
DEBUG.print("|"); DEBUG.print("|");
DEBUG.print(true_yb); DEBUG.print(true_yb);
DEBUG.print("|"); DEBUG.print("|");
DEBUG.print(true_xy); DEBUG.print(true_xy);
DEBUG.print("|"); DEBUG.print("|");
DEBUG.println(true_yy); DEBUG.println(true_yy);
DEBUG.println("---------------"); DEBUG.println("---------------");
delay(150); delay(150);
} }

5
src/goalie.cpp
View File

@ -20,8 +20,9 @@ void Goalie::init(){
} }
void Goalie::realPlay(){ void Goalie::realPlay(){
if(ball->ballSeen) this->goalie(50); ((PositionSysCamera*)ps)->goCenter();
else ((PositionSysCamera*)ps)->goCenter(); // if(ball->ballSeen) this->goalie(50);
// else ((PositionSysCamera*)ps)->goCenter();
} }
int dir, degrees2; int dir, degrees2;

22
src/positionsys_camera.cpp
View File

@ -14,11 +14,11 @@ void PositionSysCamera::test(){
void PositionSysCamera::goCenter(){ void PositionSysCamera::goCenter(){
/*WORKS BUT CAN BE BETTER*/ /*WORKS BUT CAN BE BETTER*/
//Y //Y
if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
else if ((camera->true_yb + camera->true_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
//X //X
else if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
else drive->prepareDrive(0, 0, 0); else drive->prepareDrive(0, 0, 0);
@ -27,17 +27,13 @@ void PositionSysCamera::goCenter(){
// int y = 1; // int y = 1;
// //Trying using an angle // //Trying using an angle
// if((CURRENT_DATA_READ.yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy) < -CAMERA_CENTER_Y) y = CURRENT_DATA_READ.yb + CURRENT_DATA_READ.yy; // if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y)
// if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xb; // y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
// if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xy; // if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb;
// if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy;
// DEBUG.print(x); // int dir = -90-(atan2(y*1.5,x)*180/3.14);
// DEBUG.print(":");
// DEBUG.println(y);
// int dir = 90-(atan2(y,x)*180/3.14);
// dir = (dir+360) % 360; // dir = (dir+360) % 360;
// DEBUG.println(dir);
// drive->prepareDrive(dir, 100, 0); // drive->prepareDrive(dir, 100, 0);
} }

11
utility/OpenMV/conic_eff.py
View File

@ -38,9 +38,8 @@ blue_led.on()
############################################################################## ##############################################################################
thresholds = [ (72, 100, -18, 11, 12, 65) , # thresholds yellow goal
thresholds = [ (40, 100, -14, 21, 16, 69), # thresholds yellow goal (39, 61, -18, 11, -47, -16)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
(14, 46, -11, 12, -47, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152) roi = (0, 6, 318, 152)
@ -62,9 +61,9 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3) sensor.set_contrast(+3)
sensor.set_saturation(0) sensor.set_saturation(0)
sensor.set_brightness(-1) sensor.set_brightness(-2)
sensor.set_quality(0) sensor.set_quality(0)
sensor.set_auto_exposure(False, 3000) sensor.set_auto_exposure(False, 6000)
sensor.set_auto_gain(True) sensor.set_auto_gain(True)
sensor.skip_frames(time = 300) sensor.skip_frames(time = 300)
@ -84,7 +83,7 @@ while(True):
tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, 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() img = sensor.snapshot()
for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=70, merge = True): for blob in img.find_blobs(thresholds, pixels_threshold=50, area_threshold=80, merge = True):
img.draw_rectangle(blob.rect()) img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy()) img.draw_cross(blob.cx(), blob.cy())

2
utility/OpenMV/main_test_conic.py
View File

@ -30,7 +30,7 @@ blue_led.on()
# (30, 45, 1, 40, -60, -19)] # thresholds blue goal # (30, 45, 1, 40, -60, -19)] # thresholds blue goal
# #
thresholds = [ (57, 93, -18, 14, 28, 77) , # thresholds yellow goal thresholds = [ (57, 93, -18, 14, 28, 77) , # thresholds yellow goal
(31, 68, -20, 18, -47, -17)] # thresholds blue goal (6, 31, -15, 4, -35, 0) (24, 46, -21, 6, -40, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152) roi = (0, 6, 318, 152)