Revert "ball: new ball reading algorithm, should improve precision"

This reverts commit d0324e5a93.

include/behaviour_control/status_vector.h

@@ -29,10 +29,9 @@
 #define CURRENT_INPUT_WRITE ( inputs[((currentSVIndex))] )
 
 typedef struct input{
-    int IMUAngle, USfr, USsx, USdx, USrr, BT, ballPresenceVal;
+    int IMUAngle, USfr, USsx, USdx, USrr, BT;
-    byte cameraByte, ballByte, lineByte, xb, yb, xy, yy;
+    byte ballByte, cameraByte, lineByte, xb, yb, xy, yy;
     bool SW_DX, SW_SX;
-    String ballString;
 }input;
 
 typedef struct data{

include/sensors/data_source_ball.h

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

src/sensors/data_source_ball.cpp

@@ -8,70 +8,25 @@ 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(){
-  CURRENT_INPUT_WRITE.ballString = ballString;
+  if((value & 0x01) == 1){
-  
+      distance = value;
-  bool valid_data = false;
+      ballSeen = distance == 255 ? 0 : 1;
-  String ballAngle_str, ballDist_str;
+  }else{ 
-
+    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 > 255) 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();
-    DEBUG.println("Receiving of string: " + ballString + " resulted in: ");
+    // if(ballSeen){
     DEBUG.print(angle); 
     DEBUG.print(" | ");
     DEBUG.print(angleFix); 
@@ -79,6 +34,9 @@ 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 255
+#define NCYCLES 250
 #define BROKEN  230
 #define TOO_LOW 45
 
@@ -82,7 +82,6 @@ unsigned long t = 0;
 void setup() {
   delay(1000);
 
-  Serial.begin(57600);
   Serial1.begin(57600);
 
   
@@ -191,34 +190,16 @@ void readBallInterpolation() {
   }
 }
 
-unsigned long sendtimer = 0;
-
 void sendDataInterpolation() {
-  //We must ensure that each number is expressed with three digits, adding leading zeros if necessary, to further improve transmission reliability
+  if(sending){
-  String str_angle = String((int) angle, DEC);
+    sendAngle = ((byte) (angle / 2)) & 0b11111110;
-  String str_distance = String(NCYCLES - (int) distance, DEC);
+    Serial1.write(sendAngle);
-
+  }else{
-  String sendStr_angle = "";
+    sendDistance = map(distance, 0, NCYCLES, 254, 0);
-  String sendStr_distance = "";
+    sendDistance = sendDistance |= 0b00000001;
-
+    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() {