diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index b061937..46cae7e 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -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, diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index f7eb227..3b6c98a 100644 --- a/include/sensors/data_source_ball.h +++ b/include/sensors/data_source_ball.h @@ -8,6 +8,6 @@ class DataSourceBall : public DataSource{ void postProcess() override; void test() override; - int angle, distance; + int angle, distance, angleFix; bool ballSeen; }; \ No newline at end of file diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index e8134a6..f27014b 100644 --- a/src/sensors/data_source_ball.cpp +++ b/src/sensors/data_source_ball.cpp @@ -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);