2021 NHK B
Diff: OmniPosition.cpp
- Revision:
- 6:f8dbbe93bc7b
- Parent:
- 5:f8c3aeb4e65f
- Child:
- 7:73e542a88106
--- a/OmniPosition.cpp Wed Aug 22 20:21:48 2018 +0900 +++ b/OmniPosition.cpp Fri Aug 24 14:40:25 2018 +0900 @@ -1,103 +1,37 @@ #include "OmniPosition.h" OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : - RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), - readCounter(0), - takeCounter(0), - X(0), - Y(0), - theta(0.0) + RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD) { - buffer = new char[OP_SERIAL_BUFFER_SIZE]; - data = new char[2]; - attach(callback(this, &OmniPosition::readData)); - assembleTicker.attach(callback(this, &OmniPosition::assemble), OP_RECEIVE_FREQ); + attach(callback(this, &OmniPosition::receiveByte)); + thread.start(callback(this, &OmniPosition::assembleLoop)); sendTicker.attach(callback(this, &OmniPosition::send), OP_SEND_FREQ); } -void OmniPosition::readData() +void OmniPosition::receiveByte() { - buffer[(int)readCounter] = getc(); - readCounter = incrementCounter(readCounter); + buf.push_back(getc()); } void OmniPosition::assemble() { - //Find header - headerCheck = false; - headerPoint = 0xff; + X = ((buf[2]<<8)|buf[3]) - 32768; + Y = ((buf[4]<<8)|buf[5]) - 32768; + theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00); +} - for(int i = 0; i < OP_SERIAL_BUFFER_SIZE; i++) { - if(buffer[i] == OP_HEADER_FIRST_BYTE) { - takeCounter = i; - takeCounter = incrementCounter(takeCounter); - if(buffer[(int)takeCounter] == OP_HEADER_SECOND_BYTE) { - headerCheck = true; - headerPoint = i; +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()); } } } - if(headerPoint == 0xff) { - return; - } - - //assemble - checksum = 0; - takeCounter = headerPoint; //firstheader - takeCounter = incrementCounter(takeCounter); //secondheader - - takeCounter = incrementCounter(takeCounter); //secondheader - data[0] = buffer[(int)takeCounter]; - takeCounter = incrementCounter(takeCounter); //secondheader - data[1] = buffer[(int)takeCounter]; - X = ((data[0]<<8)|data[1]) - 32768; - checksum = (data[0] ^ data[1]); - - takeCounter = incrementCounter(takeCounter); //secondheader - data[0] = buffer[(int)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 = incrementCounter(takeCounter); //secondheader - data[0] = buffer[(int)takeCounter]; - takeCounter = incrementCounter(takeCounter); //secondheader - data[1] = buffer[(int)takeCounter]; - thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00); - theta = thetaint / 100.0; - if(theta > 180.0) { - theta -= 655.0; - } - theta *= -M_PI / 180.0; - if(theta > M_PI) theta -= 2 * M_PI; - if(theta < -M_PI) theta += 2 * M_PI; - if(theta > M_PI) theta -= 2 * M_PI; - if(theta < -M_PI) theta += 2 * M_PI; - checksum = (checksum ^ data[0]); - checksum = (checksum ^ data[1]); - - takeCounter = incrementCounter(takeCounter); //secondheader - if(buffer[(int)takeCounter] != checksum) { - X = bfrX; - Y = bfrY; - theta = bfrTheta; - } else { - bfrX = X; - bfrY = Y; - bfrTheta = theta; - } - -} - -int OmniPosition::incrementCounter(int counter) -{ - ++counter; - if(counter >= OP_SERIAL_BUFFER_SIZE) { - counter -= OP_SERIAL_BUFFER_SIZE; - } - return counter; } void OmniPosition::send() @@ -110,19 +44,19 @@ } } -int OmniPosition::getX() +int16_t OmniPosition::getX() { return X; } -int OmniPosition::getY() +int16_t OmniPosition::getY() { return Y; } -double OmniPosition::getTheta() +float OmniPosition::getTheta() { - return -theta; + return (float)(theta / 100.0); } void OmniPosition::reset()