From 325656d81066995468253d76d6bcf8993b13f471 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Mon, 29 Mar 2021 15:44:19 +0200
Subject: [PATCH] ball sensor: ball angle in absolute space

---
 include/behaviour_control/status_vector.h | 2 +-
 include/sensors/data_source_ball.h        | 2 +-
 src/sensors/data_source_ball.cpp          | 8 +++++++-
 3 files changed, 9 insertions(+), 3 deletions(-)

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);