EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

aaaaaaaaaaaaaaaaA

code_midgen
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,

62
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;
@ -28,8 +29,7 @@ void DataSourceCameraConic :: readSensor(){
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;
@ -54,24 +54,13 @@ void DataSourceCameraConic :: readSensor(){
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));
}
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++;
}
}
//Important: update status vector //Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value; CURRENT_INPUT_WRITE.cameraByte = value;
CURRENT_DATA_WRITE.xb = true_xb; CURRENT_DATA_WRITE.cam_xb = true_xb;
CURRENT_DATA_WRITE.yb = true_yb; CURRENT_DATA_WRITE.cam_yb = true_yb;
CURRENT_DATA_WRITE.xy = true_xy; CURRENT_DATA_WRITE.cam_xy = true_xy;
CURRENT_DATA_WRITE.yy = true_yy; CURRENT_DATA_WRITE.cam_yy = true_yy;
CURRENT_DATA_WRITE.yAngle = yAngle; CURRENT_DATA_WRITE.yAngle = yAngle;
CURRENT_DATA_WRITE.bAngle = bAngle; CURRENT_DATA_WRITE.bAngle = bAngle;
CURRENT_DATA_WRITE.yAngleFix = yAngleFix; CURRENT_DATA_WRITE.yAngleFix = yAngleFix;
@ -79,10 +68,14 @@ void DataSourceCameraConic :: readSensor(){
CURRENT_DATA_WRITE.yDist = yDist; CURRENT_DATA_WRITE.yDist = yDist;
CURRENT_DATA_WRITE.bDist = bDist; CURRENT_DATA_WRITE.bDist = bDist;
if(xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false; if (xb == unkn || yb == unkn)
else CURRENT_DATA_WRITE.bSeen = true; CURRENT_DATA_WRITE.bSeen = false;
if(xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false; else
else CURRENT_DATA_WRITE.ySeen = true; CURRENT_DATA_WRITE.bSeen = true;
if (xy == unkn || yy == unkn)
CURRENT_DATA_WRITE.ySeen = false;
else
CURRENT_DATA_WRITE.ySeen = true;
if (goalOrientation == HIGH) { if (goalOrientation == HIGH) {
CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle; CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
@ -100,8 +93,26 @@ void DataSourceCameraConic :: readSensor(){
CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen; CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
} }
} }
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++;
}
}
}
} }
// 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;
@ -112,7 +123,8 @@ 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: ");

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)