From 5587ab02b1bed8e8f7f83af8a1c3f232ee0ba00d Mon Sep 17 00:00:00 2001 From: emamaker Date: Mon, 16 May 2022 20:39:07 +0200 Subject: [PATCH] Revert "ball: new ball reading algorithm, should improve precision" This reverts commit d0324e5a936a5343c657e2536e15725439aeb65e. --- include/behaviour_control/status_vector.h | 5 +- include/sensors/data_source_ball.h | 3 - src/sensors/data_source_ball.cpp | 70 +++++------------------ utility/32u4/ball_read/ball_read.ino | 37 +++--------- 4 files changed, 25 insertions(+), 90 deletions(-) diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 3d4ebee..a1dfcb7 100644 --- a/include/behaviour_control/status_vector.h +++ b/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; - byte cameraByte, ballByte, lineByte, xb, yb, xy, yy; + int IMUAngle, USfr, USsx, USdx, USrr, BT; + byte ballByte, cameraByte, 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 cafd297..aef29b0 100644 --- a/include/sensors/data_source_ball.h +++ b/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; }; \ No newline at end of file diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index a568e3c..548e11e 100644 --- a/src/sensors/data_source_ball.cpp +++ b/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; - - 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 > 255) valid_data = false; - } - } - - if(valid_data){ - ballSeen = distance == 255 ? 0 : 1; - + 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_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(){ diff --git a/utility/32u4/ball_read/ball_read.ino b/utility/32u4/ball_read/ball_read.ino index 6d4e4ee..9cc2a02 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 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 - 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(); + if(sending){ + sendAngle = ((byte) (angle / 2)) & 0b11111110; + Serial1.write(sendAngle); + }else{ + sendDistance = map(distance, 0, NCYCLES, 254, 0); + sendDistance = sendDistance |= 0b00000001; + Serial1.write(sendDistance); } - + sending = !sending; } void test() {