library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
Diff: OmniPosition.cpp
- Revision:
- 5:f8c3aeb4e65f
- Parent:
- 4:fc4c88fffef8
- Child:
- 6:f8dbbe93bc7b
diff -r fc4c88fffef8 -r f8c3aeb4e65f OmniPosition.cpp --- a/OmniPosition.cpp Tue Aug 21 13:41:25 2018 +0900 +++ b/OmniPosition.cpp Wed Aug 22 20:21:48 2018 +0900 @@ -1,24 +1,24 @@ #include "OmniPosition.h" OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : - RawSerial(serialTX, serialRX, DEFAULT_BAUD), - readCounter(SERIAL_BUFFER_SIZE, 0), - takeCounter(SERIAL_BUFFER_SIZE, 0), + RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), + readCounter(0), + takeCounter(0), X(0), Y(0), theta(0.0) { - buffer = new char[SERIAL_BUFFER_SIZE]; + buffer = new char[OP_SERIAL_BUFFER_SIZE]; data = new char[2]; attach(callback(this, &OmniPosition::readData)); - assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); - sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ); + assembleTicker.attach(callback(this, &OmniPosition::assemble), OP_RECEIVE_FREQ); + sendTicker.attach(callback(this, &OmniPosition::send), OP_SEND_FREQ); } void OmniPosition::readData() { buffer[(int)readCounter] = getc(); - ++readCounter; + readCounter = incrementCounter(readCounter); } void OmniPosition::assemble() @@ -27,11 +27,11 @@ headerCheck = false; headerPoint = 0xff; - for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) { - if(buffer[i] == HEADER_FIRST_BYTE) { + for(int i = 0; i < OP_SERIAL_BUFFER_SIZE; i++) { + if(buffer[i] == OP_HEADER_FIRST_BYTE) { takeCounter = i; - ++takeCounter; - if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) { + takeCounter = incrementCounter(takeCounter); + if(buffer[(int)takeCounter] == OP_HEADER_SECOND_BYTE) { headerCheck = true; headerPoint = i; } @@ -44,26 +44,26 @@ //assemble checksum = 0; takeCounter = headerPoint; //firstheader - ++takeCounter; //secondheader + takeCounter = incrementCounter(takeCounter); //secondheader - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader data[0] = buffer[(int)takeCounter]; - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader data[1] = buffer[(int)takeCounter]; X = ((data[0]<<8)|data[1]) - 32768; checksum = (data[0] ^ data[1]); - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader data[0] = buffer[(int)takeCounter]; - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader data[1] = buffer[(int)takeCounter]; Y = ((data[0]<<8)|data[1]) - 32768; checksum = (checksum ^ data[0]); checksum = (checksum ^ data[1]); - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader data[0] = buffer[(int)takeCounter]; - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader data[1] = buffer[(int)takeCounter]; thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00); theta = thetaint / 100.0; @@ -78,7 +78,7 @@ checksum = (checksum ^ data[0]); checksum = (checksum ^ data[1]); - ++takeCounter; + takeCounter = incrementCounter(takeCounter); //secondheader if(buffer[(int)takeCounter] != checksum) { X = bfrX; Y = bfrY; @@ -91,6 +91,15 @@ } +int OmniPosition::incrementCounter(int counter) +{ + ++counter; + if(counter >= OP_SERIAL_BUFFER_SIZE) { + counter -= OP_SERIAL_BUFFER_SIZE; + } + return counter; +} + void OmniPosition::send() { if(resetSend) {