hidaka sato / Mbed 2 deprecated WRS2020_mecanum_node

WRS2020用にメカナム台車をROS化

Dependencies:   mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node

Dependents:   WRS2020_mecanum_node

Diff: MyOdometer.h

Revision:
4:40167450cdf0
Parent:
0:c177452db984
--- a/MyOdometer.h	Wed Nov 11 07:34:11 2020 +0000
+++ b/MyOdometer.h	Thu Nov 12 07:36:18 2020 +0000
@@ -63,8 +63,8 @@
         {
             wheel_rad[i] = counter[i].getRadian(wheel_frequency[i]);
         }
-        float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
-        float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
+        float dx = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
+        float dy = ( wheel_rad[0] - wheel_rad[1] - wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
         
         x += dx * cos(theta) + dy * sin(-theta);
         y += dy * cos(theta) + dx * sin(theta);