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"
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0
#define CAMERA_CENTER_X 3
#define CAMERA_CENTER_Y 6
class PositionSysCamera : public PositionSystem{

3
include/status_vector.h
View File

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

106
src/data_source_camera_conicmirror.cpp
View File

@ -1,7 +1,8 @@
#include "data_source_camera_conicmirror.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_yb = 0;
true_xy = 0;
@ -21,57 +22,45 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : D
bDist = 0;
}
void DataSourceCameraConic :: readSensor(){
while(ser->available() > 0){
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;
if (value == startp) {
start = true;
count = 0;
} else if (value == endp) {
data_received = false;
if (count = 4 && start == true) {
data_received = true;
true_xb = xb-50;
true_yb = 50-yb;
true_xy = xy-50;
true_yy = 50-yy;
true_xb = xb - 50;
true_yb = 50 - yb;
true_xy = xy - 50;
true_yy = 50 - yy;
//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);
bAngle = -90-(atan2(true_yb, true_xb)* 180 /3.14);
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;
yAngle = (yAngle + 360) % 360;
bAngle = (bAngle + 360) % 360;
int angleFix = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
//Fixes with IMU
yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ;
bAngleFix = ((int) ((bAngle + angleFix*0.8)) + 360) % 360;
yAngleFix = ((int)((yAngle + 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));
bDist = sqrt((true_yb - 50) * (true_yb - 50) + (50 - true_xb) * (50 - true_xb));
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) );
}
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
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.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;
@ -79,19 +68,23 @@ void DataSourceCameraConic :: readSensor(){
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 (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){
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{
} 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;
@ -100,9 +93,27 @@ void DataSourceCameraConic :: readSensor(){
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){
// if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
// else return goalOrientation == HIGH ? yAngle : bAngle;
@ -112,7 +123,8 @@ void DataSourceCameraConic :: readSensor(){
// else return goalOrientation == LOW ? yAngle : bAngle;
// }>
void DataSourceCameraConic::test(){
void DataSourceCameraConic::test()
{
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
update();
DEBUG.print("Blue: ");

5
src/goalie.cpp
View File

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

22
src/positionsys_camera.cpp
View File

@ -14,11 +14,11 @@ void PositionSysCamera::test(){
void PositionSysCamera::goCenter(){
/*WORKS BUT CAN BE BETTER*/
//Y
if((camera->true_yb + camera->true_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);
if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
//X
else if(camera->true_xb < -CAMERA_CENTER_X || camera->true_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(90, 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);
@ -27,17 +27,13 @@ void PositionSysCamera::goCenter(){
// int y = 1;
// //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.bSeen && (CURRENT_DATA_READ.xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.xb;
// 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.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y)
// y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
// 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);
// DEBUG.print(":");
// DEBUG.println(y);
// int dir = 90-(atan2(y,x)*180/3.14);
// int dir = -90-(atan2(y*1.5,x)*180/3.14);
// dir = (dir+360) % 360;
// DEBUG.println(dir);
// drive->prepareDrive(dir, 100, 0);
}

11
utility/OpenMV/conic_eff.py
View File

@ -38,9 +38,8 @@ blue_led.on()
##############################################################################
thresholds = [ (40, 100, -14, 21, 16, 69), # thresholds yellow goal
(14, 46, -11, 12, -47, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (72, 100, -18, 11, 12, 65) , # thresholds yellow goal
(39, 61, -18, 11, -47, -16)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152)
@ -62,9 +61,9 @@ sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3)
sensor.set_saturation(0)
sensor.set_brightness(-1)
sensor.set_brightness(-2)
sensor.set_quality(0)
sensor.set_auto_exposure(False, 3000)
sensor.set_auto_exposure(False, 6000)
sensor.set_auto_gain(True)
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
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_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
#
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)