include/sensors/data_source_ball.h

@@ -7,16 +7,18 @@
 #define MOUTH_DISTANCE 100
 #define MOUTH_MAX_DISTANCE 140
 
-class DataSourceBall : public DataSource{
+class DataSourceBall : public DataSource
+{
 
-    public:
+public:
-        DataSourceBall(HardwareSerial* ser, int baud);
+    DataSourceBall(HardwareSerial *ser, int baud);
+    void readSensor() override;
     void postProcess() override;
     void test() override;
     bool isInMouth();
     bool isInMouthMaxDistance();
     bool isInFront();
 
-        int angle, distance, angleFix;
+    int angle = 0, distance = 0, angleFix = 0;
-        bool ballSeen;
+    bool ballSeen = false;
 };

src/sensors/data_source_ball.cpp

@@ -1,30 +1,42 @@
-#include "behaviour_control/status_vector.h"
 #include "sensors/data_source_ball.h"
+#include "behaviour_control/status_vector.h"
 #include "vars.h"
 
-DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(ser_, baud) {
+DataSourceBall ::DataSourceBall(HardwareSerial *ser_, int baud) : DataSource(ser_, baud)
-  ballSeen = false;
+{
-  distance = 0;
-  angle = 0;
 }
 
-void DataSourceBall :: postProcess(){
+byte buf[3] = {0, 0, 255};
-  if((value & 0x01) == 1){
+void DataSourceBall ::readSensor()
-      distance = value;
+{
-      ballSeen = distance == 255 ? 0 : 1;
+    // while(ser->available() >= 3) DEBUG.print(ser->read());
-  }else{ 
+    while (ser->available() >= 3)
-    angle = value * 2;
+        ser->readBytes(buf, 3);
+}
+
+void DataSourceBall ::postProcess()
+{
+    if ((buf[0] & 0b01000000) == 0b01000000 && (buf[1] & 0b10000000) == 0b10000000 && (buf[2] & 0b11000000) == 0b11000000)
+    {
+        angle = buf[0] & 0b00111111;
+        angle |= (buf[1] & 0b00000111) << 6;
+        distance = buf[2] & 0b00011111;
+        distance |= (buf[1] & 0b00111000) << 2;
+
+        ballSeen = distance != 255;
+
         int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle;
-    angleFix = (angle+imuAngle+360)%360;
+        angleFix = (angle + 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;
+    }
 }
 
-void DataSourceBall :: test(){
+void DataSourceBall ::test()
+{
     this->update();
     // if(ballSeen){
     DEBUG.print(angle);
@@ -34,19 +46,19 @@ void DataSourceBall :: test(){
     DEBUG.print(distance);
     DEBUG.print(" | ");
     DEBUG.println(ballSeen);
-    // }else{
-    // DEBUG.println("Not seeing ball");
-    // }
 }
 
-bool DataSourceBall::isInFront(){
+bool DataSourceBall::isInFront()
-  return CURRENT_DATA_READ.ballSeen && (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE );
+{
+    return CURRENT_DATA_READ.ballSeen && (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE);
 }
 
-bool DataSourceBall::isInMouth(){
+bool DataSourceBall::isInMouth()
-  return isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE;
+{
+    return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_DISTANCE;
 }
 
-bool DataSourceBall::isInMouthMaxDistance(){
+bool DataSourceBall::isInMouthMaxDistance()
+{
     return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE;
 }

utility/32u4/ball_read/ball_read.ino

@@ -38,30 +38,26 @@
 #define S12 ((PIND & 64) >> 6)
 #define S11 ((PIND & 128) >> 7)
 
-#define NCYCLES 250
+#define NCYCLES 255
 #define BROKEN  230
 #define TOO_LOW 45
 
-int counter[16];
+byte counter[16];
-int distance;
+byte distance;
-int nmax = 0;
+byte nmax = 0;
 int sensor = 0;
 
 int oldIndex = 0;
 int oldDistance = 0;
 
-byte ballInfo = 0;
-
 float xs[16];
 float ys[16];
 
 float angle = 0;
-int dist = 0;
+byte dist = 0;
-boolean sending = false;
-byte sendAngle = 0, sendDistance = 0;
-byte sendByte = 0;
 
 unsigned long t = 0;
+byte buf[3];
 
 void setup() {
   delay(1000);
@@ -143,7 +139,7 @@ void readBallInterpolation() {
   }
 
   angle = atan2(y, x) * 180 / PI;
-  angle = ((int)(angle + 360)) % 360;
+  angle = ( ((int)(angle) + 360)) % 360;
 
   //distance is 0 when not seeing ball
   //dist = hypot(x, y);
@@ -158,10 +154,10 @@ void readBallInterpolation() {
     }
   }
 
-  distance = nmax;
+  distance = NCYCLES - nmax;
   
   //turn led on
-  if (distance == 0) {
+  if (distance == NCYCLES) {
     digitalWrite(A4, LOW);
   } else {    
     digitalWrite(A4, HIGH);
@@ -169,15 +165,16 @@ void readBallInterpolation() {
 }
 
 void sendDataInterpolation() {
-  if(sending){
+  buf[0] = 0b01000000;
-    sendAngle = ((byte) (angle / 2)) & 0b11111110;
+  buf[1] = 0b10000000;
-    Serial.write(sendAngle);
+  buf[2] = 0b11000000;
-  }else{
+
-    sendDistance = NCYCLES - distance;
+  buf[0] |= ((int)angle) & 0b000111111;
-    sendDistance = sendDistance |= 0b00000001;
+  buf[1] |= (((int)angle) & 0b111000000) >> 6;
-    Serial.write(sendDistance);
+  buf[1] |= (distance & 0b11100000) >> 2;
-  }
+  buf[2] |= distance & 0b00011111;
-  sending = !sending;
+  
+  if(Serial.availableForWrite() >= 3) Serial.write(buf, 3);
 }
 
 void test() {