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

camera fixing and new goalie(to test)

code_midgen
u-siri-ous 2020-01-22 16:55:55 +01:00
parent 3f6835ac94
commit 13134cac99
4 changed files with 43 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/data_source_ball.h
View File

@ -10,4 +10,5 @@ class DataSourceBall : public DataSource{
int angle, distance; int angle, distance;
bool ballSeen; bool ballSeen;
int plusang, dir, degrees2,b;
}; };

2
include/data_source_camera.h
View File

@ -21,7 +21,7 @@ class DataSourceCamera : public DataSource{
int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB; int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
int cameraReady; int cameraReady;
char value; int value;
int startpY = 0; int startpY = 0;
int startpB = 0; int startpB = 0;
int endpY = 0; int endpY = 0;

31
src/data_source_camera.cpp
View File

@ -5,7 +5,8 @@ DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(
void DataSourceCamera :: readSensor(){ void DataSourceCamera :: readSensor(){
while(ser->available() > 0){ while(ser->available() > 0){
value = (byte)ser->read(); value = (int)ser->read();
//Serial.println(value);
if(value==startp){ if(value==startp){
start=true; start=true;
count=0; count=0;
@ -14,19 +15,21 @@ void DataSourceCamera :: readSensor(){
end=true; end=true;
start=false; start=false;
data_received=false; data_received=false;
if(count==3) { if(count==3 && start==true) {
data_received=true; data_received=true;
xb=true_xb; true_xb = xb;
yb=true_yb; true_yb = yb;
xy=true_xy; true_xy = xy;
yy=true_yy; true_yy = yy;
} }
}else{ }else{
if(start==true){
if (count==0) xb=value; if (count==0) xb=value;
else if (count==1) xy=value; else if (count==1) xy=value;
else if (count==2) yb=value; else if (count==2) yb=value;
else if (count==3) yy=value; else if (count==3) yy=value;
count++; count++;
}
} }
} }
} }
@ -172,8 +175,8 @@ int DataSourceCamera::getValueDef(bool fixed){
} }
void DataSourceCamera::test(){ void DataSourceCamera::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
update();
/* DEBUG.print(pAtk); /* DEBUG.print(pAtk);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.print(fixCamIMU(pAtk)); DEBUG.print(fixCamIMU(pAtk));
@ -182,28 +185,24 @@ void DataSourceCamera::test(){
DEBUG.print(pDef); DEBUG.print(pDef);
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.println(fixCamIMU(pDef)); */ DEBUG.println(fixCamIMU(pDef)); */
update(); //update();
DEBUG.print(xb); DEBUG.print(xb);
DEBUG.print("|"); DEBUG.print("|");
delay(100);
DEBUG.print(yb); DEBUG.print(yb);
DEBUG.print("|"); DEBUG.print("|");
delay(100);
DEBUG.print(xy); DEBUG.print(xy);
DEBUG.print("|"); DEBUG.print("|");
delay(100);
DEBUG.print(yy); DEBUG.print(yy);
DEBUG.println("---------------"); DEBUG.println("---------------");
DEBUG.print(true_xb); DEBUG.print(true_xb);
DEBUG.print("|"); DEBUG.print("|");
delay(100);
DEBUG.print(true_yb); DEBUG.print(true_yb);
DEBUG.print("|"); DEBUG.print("|");
delay(100);
DEBUG.print(true_xy); DEBUG.print(true_xy);
DEBUG.print("|"); DEBUG.print("|");
delay(100);
DEBUG.print(true_yy); DEBUG.print(true_yy);
DEBUG.println("---------------");
delay(75);
} }
int DataSourceCamera::fixCamIMU(int d){ int DataSourceCamera::fixCamIMU(int d){

38
src/goalie.cpp
View File

@ -22,7 +22,20 @@ void Goalie::realPlay(){
} }
void Goalie::goalie() { void Goalie::goalie() {
if(ball->angle >= 350 || ball->angle <= 10) { if(ball->angle > 340 || ball->angle < 20) ball->plusang -= 20;
if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
else ball->degrees2 = ball->angle;
if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang; //45 con 8 ruote
else ball->dir = ball->angle - ball->plusang; //45 con 8 ruote
if(ball->dir < 0) ball->dir = ball->dir + 360;
else ball->dir = ball->dir;
ball->b = ball->dir;
drive->prepareDrive(ball->dir, 300, 0);
/* drive->speed = 300;
drive->dir = drive->dir; */
/* if(ball->angle >= 350 || ball->angle <= 10) {
if(ball->distance > 190) atk_direction = 0; if(ball->distance > 190) atk_direction = 0;
else atk_direction = ball->angle; else atk_direction = ball->angle;
atk_speed = GOALIE_ATKSPD_FRT; atk_speed = GOALIE_ATKSPD_FRT;
@ -62,7 +75,7 @@ void Goalie::goalie() {
atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito
drive->prepareDrive(atk_direction, atk_speed, cstorc); drive->prepareDrive(atk_direction, atk_speed, cstorc);
} }
else drive->prepareDrive(atk_direction, atk_speed); else drive->prepareDrive(atk_direction, atk_speed); */
} }
void Goalie::storcimentoPorta() { void Goalie::storcimentoPorta() {
@ -73,18 +86,15 @@ void Goalie::storcimentoPorta() {
} }
void Goalie::ballBack() { void Goalie::ballBack() {
int degrees2;
int dir;
int plusang; if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK;
if(ball->distance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; else ball->plusang = 0;
else plusang = 0;
if(ball->angle > 180) degrees2 = ball->angle - 360; if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
else degrees2 = ball->angle; else ball->degrees2 = ball->angle;
if(degrees2 > 0) dir = ball->angle + plusang; //45 con 8 ruote if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang; //45 con 8 ruote
else dir = ball->angle - plusang; //45 con 8 ruote else ball->dir = ball->angle - ball->plusang; //45 con 8 ruote
if(dir < 0) dir = dir + 360; if(ball->dir < 0) ball->dir = ball->dir + 360;
else dir = dir; else ball->dir = ball->dir;
atk_direction = dir; atk_direction = ball->dir;
} }