2021 NHK B
Diff: OmniPosition.cpp
- Revision:
- 3:47676abdf529
- Parent:
- 2:edd33d3ad0fd
- Child:
- 4:fc4c88fffef8
--- a/OmniPosition.cpp Sun Jul 01 16:39:13 2018 +0900 +++ b/OmniPosition.cpp Tue Jul 31 08:30:35 2018 +0000 @@ -4,11 +4,10 @@ RawSerial(serialTX, serialRX, DEFAULT_BAUD), readCounter(SERIAL_BUFFER_SIZE, 0), takeCounter(SERIAL_BUFFER_SIZE, 0), - X(0), - Y(0) + angle(0) { - buffer = new char[SERIAL_BUFFER_SIZE]; - data = new char[2]; + buffer = new uint8_t[SERIAL_BUFFER_SIZE]; + data = new uint8_t[2]; attach(callback(this, &OmniPosition::readData)); assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ); @@ -45,16 +44,16 @@ ++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; + angleInt = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00); + angle = angleInt/100.0; + if(angle > 180){ + angle = angle - 655; + } + } void OmniPosition::send() @@ -67,15 +66,11 @@ } } -int OmniPosition::getX() +double OmniPosition::getAngle() { - return X; + return angle; } -int OmniPosition::getY() -{ - return Y; -} void OmniPosition::reset() {