library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
Diff: OmniPosition.cpp
- Revision:
- 2:edd33d3ad0fd
- Parent:
- 1:0229fc98a26f
- Child:
- 3:47676abdf529
--- a/OmniPosition.cpp Sat Jun 30 14:23:07 2018 +0900 +++ b/OmniPosition.cpp Sun Jul 01 16:39:13 2018 +0900 @@ -3,11 +3,15 @@ OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : RawSerial(serialTX, serialRX, DEFAULT_BAUD), readCounter(SERIAL_BUFFER_SIZE, 0), - takeCounter(SERIAL_BUFFER_SIZE, 0) + takeCounter(SERIAL_BUFFER_SIZE, 0), + X(0), + Y(0) { buffer = new char[SERIAL_BUFFER_SIZE]; + data = new char[2]; attach(callback(this, &OmniPosition::readData)); - ticker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); + assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); + sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ); } void OmniPosition::readData() @@ -39,4 +43,41 @@ //assemble takeCounter = headerPoint; //firstheader ++takeCounter; //secondheader + + ++takeCounter; + data[0] = buffer[(int)takeCounter]; + ++takeCounter; + data[1] = buffer[(int)takeCounter]; + X = ((data[0]<<8)|data[1]) - 32768; + + ++takeCounter; + data[0] = buffer[(int)takeCounter]; + ++takeCounter; + data[1] = buffer[(int)takeCounter]; + Y = ((data[0]<<8)|data[1]) - 32768; } + +void OmniPosition::send() +{ + if(resetSend) { + putc('R'); + resetSend = false; + } else { + //putc(0); + } +} + +int OmniPosition::getX() +{ + return X; +} + +int OmniPosition::getY() +{ + return Y; +} + +void OmniPosition::reset() +{ + resetSend = true; +}