ROBOT_DEMO: correct speed computation

Correct calculation of speed sent to motors when moving in reverse.

diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
index 746264ee2a9e9d9cb0f0ace37158067fb531075c..d9a985076a16af13a62a7d7100a0044d45b01fd0 100644 (file)
--- a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
+++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
@@ -60,8 +60,8 @@ public class MotionSpeedPublish implements Runnable {
                }
                omega *= 2;
                 
-               speed[0] = (short)(-((v + r*omega))*VMAX);
-               speed[1] = (short)(-(v - r*omega)*VMAX);
+               speed[0] = (short)(-(v + (v>0 ? -r*omega : r*omega))*VMAX);
+               speed[1] = (short)(-(v - (v>0 ? -r*omega : r*omega))*VMAX);
        }
        
        public void cancel() {