EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

ball: new ball reading algorithm, should improve precision

pull/1/head
EmaMaker 2021-05-29 15:19:41 +02:00
parent 4397d602cd
commit d0324e5a93
5 changed files with 90 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/behaviour_control/status_vector.h
View File

@ -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{

3
include/sensors/data_source_ball.h
View File

@ -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;
};

70
src/sensors/data_source_ball.cpp
View File

@ -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(){

37
utility/32u4/ball_read/ball_read.ino
View File

@ -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() {