library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
Diff: OmniPosition.cpp
- Revision:
- 8:930369f241fb
- Parent:
- 7:73e542a88106
diff -r 73e542a88106 -r 930369f241fb OmniPosition.cpp --- a/OmniPosition.cpp Fri Aug 24 14:48:35 2018 +0900 +++ b/OmniPosition.cpp Fri Aug 24 15:03:39 2018 +0900 @@ -14,9 +14,12 @@ void OmniPosition::assemble() { - X = ((buf[2]<<8)|buf[3]) - 32768; - Y = ((buf[4]<<8)|buf[5]) - 32768; - theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00); + unsigned char checksum = 0; + if(((unsigned char)(buf[2] + buf[3] + buf[4] + buf[5] + buf[6] + buf[7])) == buf[8]) { + X = ((buf[2]<<8)|buf[3]) - 32768; + Y = ((buf[4]<<8)|buf[5]) - 32768; + theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00); + } } void OmniPosition::assembleLoop()