From d0324e5a936a5343c657e2536e15725439aeb65e Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sat, 29 May 2021 15:19:41 +0200 Subject: [PATCH] ball: new ball reading algorithm, should improve precision --- include/behaviour_control/status_vector.h | 3 +- include/sensors/data_source_ball.h | 3 + src/main.cpp | 2 +- src/sensors/data_source_ball.cpp | 70 ++++++++++++++++++----- utility/32u4/ball_read/ball_read.ino | 37 +++++++++--- 5 files changed, 90 insertions(+), 25 deletions(-) diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 8c068c0..db63b3a 100644 --- a/include/behaviour_control/status_vector.h +++ b/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{ diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index aef29b0..cafd297 100644 --- a/include/sensors/data_source_ball.h +++ b/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; }; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 2bb8c07..565c08e 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -58,7 +58,7 @@ void loop() { striker->play(1); testmenu->testMenu(); - + // Last thing to do: movement and update status vector drive->drivePrepared(); updateStatusVector(); diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index 548e11e..888a9c6 100644 --- a/src/sensors/data_source_ball.cpp +++ b/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){ - distance = value; - ballSeen = distance == 255 ? 0 : 1; - }else{ - angle = value * 2; + CURRENT_INPUT_WRITE.ballString = ballString; + + bool valid_data = false; + String ballAngle_str, ballDist_str; + + 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(){ diff --git a/utility/32u4/ball_read/ball_read.ino b/utility/32u4/ball_read/ball_read.ino index 9cc2a02..6d4e4ee 100644 --- a/utility/32u4/ball_read/ball_read.ino +++ b/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){ - sendAngle = ((byte) (angle / 2)) & 0b11111110; - Serial1.write(sendAngle); - }else{ - sendDistance = map(distance, 0, NCYCLES, 254, 0); - sendDistance = sendDistance |= 0b00000001; - Serial1.write(sendDistance); + //We must ensure that each number is expressed with three digits, adding leading zeros if necessary, to further improve transmission reliability + String str_angle = String((int) angle, DEC); + String str_distance = String(NCYCLES - (int) distance, DEC); + + String sendStr_angle = ""; + String sendStr_distance = ""; + + 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() {