ball: new ball reading algorithm, should improve precision

include/behaviour_control/status_vector.h

@@ -30,8 +30,9 @@
 
 typedef struct input{
     int IMUAngle, USfr, USsx, USdx, USrr, BT;
-    byte ballByte, cameraByte, lineByte, xb, yb, xy, yy;
+    byte cameraByte, ballByte, lineByte, xb, yb, xy, yy;
     bool SW_DX, SW_SX;
+    String ballString;
 }input;
 
 typedef struct data{

include/sensors/data_source_ball.h

@@ -11,6 +11,7 @@ class DataSourceBall : public DataSource{
 
     public:
         DataSourceBall(HardwareSerial* ser, int baud);
+        void readSensor() override;
         void postProcess() override;
         void test() override;
         bool isInMouth();
@@ -19,4 +20,6 @@ class DataSourceBall : public DataSource{
         
         int angle, distance, angleFix;
         bool ballSeen;
+
+        String ballString;
 }; 

src/sensors/data_source_ball.cpp

@@ -8,25 +8,70 @@ DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(se
   angle = 0;
 }
 
+// bool startAng = false, endAng= false, startDist = false, endDist = false;
+// String valAngle;
+// String valDist;
+
+// char valueChar;
+
+void DataSourceBall::readSensor(){
+  while(ser->available()) ballString = ser->readStringUntil('B');
+}
+
 void DataSourceBall :: postProcess(){
-  if((value & 0x01) == 1){
+  CURRENT_INPUT_WRITE.ballString = ballString;
-      distance = value;
+  
-      ballSeen = distance == 255 ? 0 : 1;
+  bool valid_data = false;
-  }else{ 
+  String ballAngle_str, ballDist_str;
-    angle = value * 2;
+
+  if(ballString.length() == 9 && ballString.startsWith('b') && ballString.endsWith('I')){                         //String has a fixed length of 9 (3*2 for data, 3 for packet recognition)
+    
+    //Try to validate data data, extract data from String
+    valid_data = true;
+
+    //String is in the form of bXXX-YYYB, with XXX being ball angle and YYY being ball distance
+    ballAngle_str = ballString.substring(ballString.lastIndexOf('b')+1, ballString.lastIndexOf('-'));
+    ballDist_str = ballString.substring(ballString.lastIndexOf('-')+1, ballString.lastIndexOf('I'));
+
+    
+    // Check if strings only contain digits
+    for(signed int i = 0; i < ballAngle_str.length(); i++)
+      if (!isDigit(ballAngle_str.charAt(i))) valid_data = false;
+    for(signed int i = 0; i < ballDist_str.length(); i++)
+      if (!isDigit(ballDist_str.charAt(i))) valid_data = false;
+
+    if(valid_data){
+      angle = ballAngle_str.toInt();
+      distance = ballDist_str.toInt();
+
+      // Invalid data if out of parameters
+      if(angle < 0 || angle >= 360 || distance < 0 || distance > 250) valid_data = false;
+    }
+  }
+
+  if(valid_data){
+    ballSeen = distance == 255 ? 0 : 1;
+
     int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
     angleFix = (angle+imuAngle+360)%360;
+    
+    CURRENT_DATA_WRITE.ballAngle = angle;
+    CURRENT_DATA_WRITE.ballAngleFix = angleFix;
+    CURRENT_DATA_WRITE.ballDistance = distance;
+    CURRENT_DATA_WRITE.ballSeen = ballSeen;
+  }else{
+    // Invalid data, recover from last loop and clear the buffer to prevent corruption of following data
+    CURRENT_DATA_WRITE.ballAngle = CURRENT_DATA_READ.ballAngle;
+    CURRENT_DATA_WRITE.ballAngleFix =  CURRENT_DATA_READ.ballAngleFix;
+    CURRENT_DATA_WRITE.ballDistance =  CURRENT_DATA_READ.ballDistance;
+    CURRENT_DATA_WRITE.ballSeen =  CURRENT_DATA_READ.ballSeen;
   }
-  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;
 }
 
 void DataSourceBall :: test(){
     this->update();
-    // if(ballSeen){
+    DEBUG.println("Receiving of string: " + ballString + " resulted in: ");
     DEBUG.print(angle); 
     DEBUG.print(" | ");
     DEBUG.print(angleFix); 
@@ -34,9 +79,6 @@ void DataSourceBall :: test(){
     DEBUG.print(distance); 
     DEBUG.print(" | ");
     DEBUG.println(ballSeen); 
-    // }else{
-    // DEBUG.println("Not seeing ball");
-    // }
 }
 
 bool DataSourceBall::isInFront(){

utility/32u4/ball_read/ball_read.ino

@@ -41,7 +41,7 @@
 #define S9 ((PINF & 64 ) >> 6)
 #define S10 ((PINF & 128 ) >> 7)
 
-#define NCYCLES 250
+#define NCYCLES 255
 #define BROKEN  230
 #define TOO_LOW 45
 
@@ -82,6 +82,7 @@ unsigned long t = 0;
 void setup() {
   delay(1000);
 
+  Serial.begin(57600);
   Serial1.begin(57600);
 
   
@@ -190,16 +191,34 @@ void readBallInterpolation() {
   }
 }
 
+unsigned long sendtimer = 0;
+
 void sendDataInterpolation() {
-  if(sending){
+  //We must ensure that each number is expressed with three digits, adding leading zeros if necessary, to further improve transmission reliability
-    sendAngle = ((byte) (angle / 2)) & 0b11111110;
+  String str_angle = String((int) angle, DEC);
-    Serial1.write(sendAngle);
+  String str_distance = String(NCYCLES - (int) distance, DEC);
-  }else{
+
-    sendDistance = map(distance, 0, NCYCLES, 254, 0);
+  String sendStr_angle = "";
-    sendDistance = sendDistance |= 0b00000001;
+  String sendStr_distance = "";
-    Serial1.write(sendDistance);
+
+  if(str_angle.length() == 1) sendStr_angle = "00" + str_angle;
+  else if(str_angle.length() == 2) sendStr_angle = "0" + str_angle;
+  else sendStr_angle = str_angle;
+  
+  if(str_distance.length() == 1) sendStr_distance = "00" + str_distance;
+  else if(str_distance.length() == 2) sendStr_distance = "0" + str_distance;
+  else sendStr_distance = str_distance;
+
+  //slow down the communciation a little bit, or it might get very messy
+  if(millis() - sendtimer > 40){
+    Serial1.print("b");
+    Serial1.print(sendStr_angle);
+    Serial1.print("-");
+    Serial1.print(sendStr_distance);
+    Serial1.print("IB");
+    sendtimer = millis();
   }
-  sending = !sending;
+
 }
 
 void test() {