library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
Diff: OmniPosition.cpp
- Revision:
- 9:222f7fcbd05a
- Parent:
- 7:73e542a88106
- Child:
- 10:a21aa2bd05c5
- Child:
- 12:edd4217ad7a5
diff -r 73e542a88106 -r 222f7fcbd05a OmniPosition.cpp --- a/OmniPosition.cpp Fri Aug 24 14:48:35 2018 +0900 +++ b/OmniPosition.cpp Tue Sep 18 15:25:05 2018 +0000 @@ -1,35 +1,23 @@ #include "OmniPosition.h" OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : - RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD) -{ - attach(callback(this, &OmniPosition::receiveByte)); - thread.start(callback(this, &OmniPosition::assembleLoop)); -} - -void OmniPosition::receiveByte() + serial(serialTX, serialRX) { - buf.push_back(getc()); -} - -void OmniPosition::assemble() -{ - X = ((buf[2]<<8)|buf[3]) - 32768; - Y = ((buf[4]<<8)|buf[5]) - 32768; - theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00); + thread.start(callback(this, &OmniPosition::assembleLoop)); + serial.baud(115200); + serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER); + serial.startReceive(BUFFER_SIZE); } void OmniPosition::assembleLoop() { while(true) { - if(buf.size() > OP_SERIAL_BUFFER_SIZE) { - if(buf[0] == OP_HEADER_FIRST_BYTE && buf[1] == OP_HEADER_SECOND_BYTE) { - assemble(); - buf.erase(buf.begin(), buf.begin() + (OP_SERIAL_BUFFER_SIZE - 1)); - } else { - buf.erase(buf.begin()); - } - } + 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]; } } @@ -52,7 +40,7 @@ { resetSend = true; if(resetSend) { - putc('R'); +// putc('R'); resetSend = false; } else { //putc(0);