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: robot_operator.h

Revision:
3:cf06189a7511
Parent:
1:b20c9bcfb254
Child:
4:40167450cdf0
--- a/robot_operator.h	Mon Nov 02 11:29:15 2020 +0000
+++ b/robot_operator.h	Wed Nov 11 07:34:11 2020 +0000
@@ -139,8 +139,11 @@
     float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot);
     float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot);
     float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot);
-    path++;
-    if(path >= pathSize) path = pathSize-1;
+    
+    if(path < pathSize-1)
+        path++;
+    else 
+        path = pathSize-1;
     
     // ロボット座標での速度
     float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;