From c27b64922cd51e36e254626bd32d14f1f400eb6d Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sun, 3 Jul 2022 13:15:09 +0200 Subject: [PATCH] ball: switch to using a buffer of 3 bytes * now we don't lose in resolution * each byte use the two MSBs as an identifier, so that information can't be misinterpreted --- include/sensors/data_source_ball.h | 26 +++++----- src/sensors/data_source_ball.cpp | 78 ++++++++++++++++------------ utility/32u4/ball_read/ball_read.ino | 41 +++++++-------- 3 files changed, 78 insertions(+), 67 deletions(-) diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index 70abefa..7e84456 100644 --- a/include/sensors/data_source_ball.h +++ b/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: - DataSourceBall(HardwareSerial* ser, int baud); - void postProcess() override; - void test() override; - bool isInMouth(); - bool isInMouthMaxDistance(); - bool isInFront(); - - int angle, distance, angleFix; - bool ballSeen; -}; \ No newline at end of file +public: + DataSourceBall(HardwareSerial *ser, int baud); + void readSensor() override; + void postProcess() override; + void test() override; + bool isInMouth(); + bool isInMouthMaxDistance(); + bool isInFront(); + + int angle = 0, distance = 0, angleFix = 0; + bool ballSeen = false; +}; \ No newline at end of file diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index 548e11e..11a6705 100644 --- a/src/sensors/data_source_ball.cpp +++ b/src/sensors/data_source_ball.cpp @@ -1,52 +1,64 @@ -#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) { - ballSeen = false; - distance = 0; - angle = 0; +DataSourceBall ::DataSourceBall(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) +{ } -void DataSourceBall :: postProcess(){ - if((value & 0x01) == 1){ - distance = value; - ballSeen = distance == 255 ? 0 : 1; - }else{ - angle = value * 2; - int imuAngle = CURRENT_DATA_READ.IMUAngle > 180 ? CURRENT_DATA_READ.IMUAngle - 360 : CURRENT_DATA_READ.IMUAngle; - 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; +byte buf[3] = {0, 0, 255}; +void DataSourceBall ::readSensor() +{ + // while(ser->available() >= 3) DEBUG.print(ser->read()); + while (ser->available() >= 3) + ser->readBytes(buf, 3); } -void DataSourceBall :: test(){ +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; + + 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.print(angle); + DEBUG.print(angle); DEBUG.print(" | "); - DEBUG.print(angleFix); + DEBUG.print(angleFix); DEBUG.print(" | "); - DEBUG.print(distance); + DEBUG.print(distance); DEBUG.print(" | "); - DEBUG.println(ballSeen); - // }else{ - // DEBUG.println("Not seeing ball"); - // } + DEBUG.println(ballSeen); } -bool DataSourceBall::isInFront(){ - return CURRENT_DATA_READ.ballSeen && (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE ); +bool DataSourceBall::isInFront() +{ + return CURRENT_DATA_READ.ballSeen && (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE); } -bool DataSourceBall::isInMouth(){ - return isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE; +bool DataSourceBall::isInMouth() +{ + return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_DISTANCE; } -bool DataSourceBall::isInMouthMaxDistance(){ - return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE; +bool DataSourceBall::isInMouthMaxDistance() +{ + return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE; } \ No newline at end of file diff --git a/utility/32u4/ball_read/ball_read.ino b/utility/32u4/ball_read/ball_read.ino index 780fdb3..4ddb27e 100644 --- a/utility/32u4/ball_read/ball_read.ino +++ b/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]; -int distance; -int nmax = 0; +byte counter[16]; +byte distance; +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; -boolean sending = false; -byte sendAngle = 0, sendDistance = 0; -byte sendByte = 0; +byte dist = 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){ - sendAngle = ((byte) (angle / 2)) & 0b11111110; - Serial.write(sendAngle); - }else{ - sendDistance = NCYCLES - distance; - sendDistance = sendDistance |= 0b00000001; - Serial.write(sendDistance); - } - sending = !sending; + buf[0] = 0b01000000; + buf[1] = 0b10000000; + buf[2] = 0b11000000; + + buf[0] |= ((int)angle) & 0b000111111; + buf[1] |= (((int)angle) & 0b111000000) >> 6; + buf[1] |= (distance & 0b11100000) >> 2; + buf[2] |= distance & 0b00011111; + + if(Serial.availableForWrite() >= 3) Serial.write(buf, 3); } void test() { -- 2.40.1