2021 NHK B
Diff: OmniPosition.cpp
- Revision:
- 10:a21aa2bd05c5
- Parent:
- 9:222f7fcbd05a
- Child:
- 11:7028941a60c1
diff -r 222f7fcbd05a -r a21aa2bd05c5 OmniPosition.cpp --- a/OmniPosition.cpp Tue Sep 18 15:25:05 2018 +0000 +++ b/OmniPosition.cpp Sat Oct 06 05:00:58 2018 +0000 @@ -1,49 +1,62 @@ #include "OmniPosition.h" -OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : - serial(serialTX, serialRX) -{ - thread.start(callback(this, &OmniPosition::assembleLoop)); - serial.baud(115200); - serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER); - serial.startReceive(BUFFER_SIZE); +OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) + : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PB_8) { + attach(callback(this, &OmniPosition::receiveByte)); + X = 0; + Y = 0; + bufferSize = 9; + bufferPoint = 0; + receivedBytes = 0; } -void OmniPosition::assembleLoop() -{ - while(true) { - serial.getData(rxdata); - for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768; - data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00); - X = data[0]; - Y = data[1]; - theta = data[2]; - } +void OmniPosition::receiveByte() { + buffer[bufferPoint % bufferSize] = getc(); + if (bufferPoint != 0xff) { + ++bufferPoint; + } else { + bufferPoint = (255 % bufferSize) + 1; + } + + ++receivedBytes; + + if (receivedBytes >= bufferSize) { + checkData(); + } } -int16_t OmniPosition::getX() -{ - return X; -} - -int16_t OmniPosition::getY() -{ - return Y; +void OmniPosition::checkData() { + for (int i = 0; i < bufferSize; i++) { + if (buffer[i % bufferSize] == OP_HEADER_FIRST_BYTE && + buffer[(i + 1) % bufferSize] == OP_HEADER_SECOND_BYTE) { + uint8_t checksum = 0; + for (int j = 0; j < bufferSize - 3; j++) { + checksum += buffer[(i + 2 + j) % bufferSize]; + } + if ((uint8_t)checksum == buffer[(i + bufferSize - 1) % bufferSize]) { + debugled = !debugled; + X = ((buffer[(i + 2 + 0) % bufferSize] << 8) | + buffer[(i + 2 + 1) % bufferSize]) - + 32768; + Y = ((buffer[(i + 2 + 2) % bufferSize] << 8) | + buffer[(i + 2 + 3) % bufferSize]) - + 32768; + theta = (buffer[(i + 2 + 4) % bufferSize] & 0xFF) | + ((buffer[(i + 2 + 5) % bufferSize] << 8) & 0xFF00); + receivedBytes = 0; + return; + } + } + } } -float OmniPosition::getTheta() -{ - return (float)(theta / 100.0); +int16_t OmniPosition::getX() { return X; } + +int16_t OmniPosition::getY() { return Y; } + +float OmniPosition::getTheta() { + return (float)(theta / 100.0) * (M_PI / 180.0); } -void OmniPosition::reset() -{ - resetSend = true; - if(resetSend) { -// putc('R'); - resetSend = false; - } else { - //putc(0); - } -} +void OmniPosition::reset() { putc('R'); }