NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

Diff: OmniPosition.cpp

Revision:
8:930369f241fb
Parent:
7:73e542a88106
--- 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()