2019
Dependents: wheel_test6 wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_red
OmniPosition.cpp
- Committer:
- takeuchi
- Date:
- 2018-08-24
- Revision:
- 7:73e542a88106
- Parent:
- 6:f8dbbe93bc7b
- Child:
- 8:930369f241fb
- Child:
- 9:222f7fcbd05a
File content as of revision 7:73e542a88106:
#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() { 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); } 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()); } } } } int16_t OmniPosition::getX() { return X; } int16_t OmniPosition::getY() { return Y; } float OmniPosition::getTheta() { return (float)(theta / 100.0); } void OmniPosition::reset() { resetSend = true; if(resetSend) { putc('R'); resetSend = false; } else { //putc(0); } }