library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
Revision 13:913b647071a8, committed 2018-10-23
- Comitter:
- UCHITAKE
- Date:
- Tue Oct 23 19:29:35 2018 +0900
- Parent:
- 12:edd4217ad7a5
- Commit message:
- tukaeru
Changed in this revision
OmniPosition.cpp | Show annotated file Show diff for this revision Revisions of this file |
OmniPosition.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r edd4217ad7a5 -r 913b647071a8 OmniPosition.cpp --- a/OmniPosition.cpp Fri Oct 12 04:38:04 2018 +0000 +++ b/OmniPosition.cpp Tue Oct 23 19:29:35 2018 +0900 @@ -1,55 +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(PA_11) { + 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() -{ - 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]; - 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'); }
diff -r edd4217ad7a5 -r 913b647071a8 OmniPosition.h --- a/OmniPosition.h Fri Oct 12 04:38:04 2018 +0000 +++ b/OmniPosition.h Tue Oct 23 19:29:35 2018 +0900 @@ -3,38 +3,34 @@ #include "mbed.h" -#include "SerialMultiByte.h" +#define OP_SERIAL_BUFFER_SIZE 9 +#define OP_HEADER_FIRST_BYTE 72 +#define OP_HEADER_SECOND_BYTE 42 +#define OP_DEFAULT_BAUD 115200 -#define FIRST_HEDDER 0xEE -#define SECOND_HEDDER 0xFF -#define BUFFER_SIZE 6 -#define TWO_BYTE_DATA 3 -#define ONE_BYTE_DATA 0 +class OmniPosition : public RawSerial { + public: + OmniPosition(PinName serialTX, PinName serialRX); -class OmniPosition -{ -public : - OmniPosition(PinName serialTX, PinName serialRX); + int16_t getX(); + int16_t getY(); + float getTheta(); - int16_t getX(); - int16_t getY(); - float getTheta(); + void reset(); - void reset(); - -private : - void assembleLoop(); + private: + DigitalOut debugled; + void receiveByte(); + void checkData(); - Thread thread; - - int16_t X; - int16_t Y; - int16_t theta; - - bool resetSend; - uint8_t rxdata[BUFFER_SIZE]; - int data[TWO_BYTE_DATA + ONE_BYTE_DATA]; - SerialMultiByte serial; + uint8_t buffer[9]; + uint8_t bufferSize; + uint8_t bufferPoint; + uint8_t receivedBytes; + int16_t X; + int16_t Y; + int16_t theta; }; -#endif \ No newline at end of file +#endif +