2021 NHK B
Diff: OmniPosition.cpp
- Revision:
- 4:fc4c88fffef8
- Parent:
- 3:47676abdf529
- Child:
- 5:f8c3aeb4e65f
--- a/OmniPosition.cpp Tue Jul 31 08:30:35 2018 +0000 +++ b/OmniPosition.cpp Tue Aug 21 13:41:25 2018 +0900 @@ -4,10 +4,12 @@ RawSerial(serialTX, serialRX, DEFAULT_BAUD), readCounter(SERIAL_BUFFER_SIZE, 0), takeCounter(SERIAL_BUFFER_SIZE, 0), - angle(0) + X(0), + Y(0), + theta(0.0) { - buffer = new uint8_t[SERIAL_BUFFER_SIZE]; - data = new uint8_t[2]; + buffer = new char[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); @@ -40,19 +42,52 @@ } //assemble + checksum = 0; takeCounter = headerPoint; //firstheader ++takeCounter; //secondheader ++takeCounter; + data[0] = buffer[(int)takeCounter]; + ++takeCounter; + data[1] = buffer[(int)takeCounter]; + X = ((data[0]<<8)|data[1]) - 32768; + checksum = (data[0] ^ data[1]); + + ++takeCounter; + data[0] = buffer[(int)takeCounter]; + ++takeCounter; + data[1] = buffer[(int)takeCounter]; + Y = ((data[0]<<8)|data[1]) - 32768; + checksum = (checksum ^ data[0]); + checksum = (checksum ^ data[1]); + ++takeCounter; data[0] = buffer[(int)takeCounter]; ++takeCounter; data[1] = buffer[(int)takeCounter]; - angleInt = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00); - angle = angleInt/100.0; - if(angle > 180){ - angle = angle - 655; - } + 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; + if(buffer[(int)takeCounter] != checksum) { + X = bfrX; + Y = bfrY; + theta = bfrTheta; + } else { + bfrX = X; + bfrY = Y; + bfrTheta = theta; + } } @@ -66,13 +101,23 @@ } } -double OmniPosition::getAngle() +int OmniPosition::getX() { - return angle; + return X; } +int OmniPosition::getY() +{ + return Y; +} + +double OmniPosition::getTheta() +{ + return -theta; +} void OmniPosition::reset() { resetSend = true; } +