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

ball sensor: ball angle in absolute space

pull/1/head
EmaMaker 2021-03-29 15:44:19 +02:00
parent f26cbf4984
commit 325656d810
3 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/behaviour_control/status_vector.h
View File

@ -35,7 +35,7 @@ typedef struct input{
}input;
typedef struct data{
int IMUAngle, ballAngle, ballDistance,
int IMUAngle, ballAngle, ballAngleFix, ballDistance,
yAngle, bAngle, yAngleFix, bAngleFix,
yDist, bDist,
angleAtk, angleAtkFix, angleDef, angleDefFix,

2
include/sensors/data_source_ball.h
View File

@ -8,6 +8,6 @@ class DataSourceBall : public DataSource{
void postProcess() override;
void test() override;
int angle, distance;
int angle, distance, angleFix;
bool ballSeen;
};

8
src/sensors/data_source_ball.cpp
View File

@ -13,10 +13,14 @@ void DataSourceBall :: postProcess(){
distance = value;
ballSeen = distance == 255 ? 0 : 1;
}else{
angle = value * 2;
angle = value * 2;
int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? 360 -CURRENT_DATA_READ.IMUAngle : CURRENT_DATA_READ.IMUAngle;
int ballAngle = angle > 180 ? 360 -angle : angle;
angleFix = (ballAngle-imuAngle+360)%360;
}
CURRENT_INPUT_WRITE.ballByte = value;
CURRENT_DATA_WRITE.ballAngle = angle;
CURRENT_DATA_WRITE.ballAngleFix = angleFix;
CURRENT_DATA_WRITE.ballDistance = distance;
CURRENT_DATA_WRITE.ballSeen = ballSeen;
}
@ -26,6 +30,8 @@ void DataSourceBall :: test(){
// if(ballSeen){
DEBUG.print(angle);
DEBUG.print(" | ");
DEBUG.print(angleFix);
DEBUG.print(" | ");
DEBUG.print(distance);
DEBUG.print(" | ");
DEBUG.println(ballSeen);