Diff: Traxster.cpp

Revision:

0:345f76c72b9a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Traxster.cpp	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,46 @@
+
+#include "Traxster.h"
+#include "utils.h"
+#include "rtos.h"
+
+//extern Serial pc;
+//extern Mutex m;
+
+int getMotorSpeedInt(float f){
+    
+    int max = 40;
+    
+    if(f > 1.0)
+        f = 1.0;
+
+    if(f < -1.0)
+        f = -1.0;
+    
+    return   (int)round( (float)max * f );
+}
+
+void Traxster::SetMotors(float fm1, float fm2)
+{
+    int m1 = getMotorSpeedInt(fm1);
+    int m2 = getMotorSpeedInt(fm2);
+    
+    //clear robot msgs
+    while (rob.readable())
+        rob.getc();
+        
+    if (m1 == 0 && m2 == 0)
+    {
+        rob.puts("stop\r");
+    }
+    else
+    {
+        //m.lock();
+        //pc.printf("mogo 1:%d 2:%d\r\n", m1, m2);
+        //m.unlock();
+        rob.printf("mogo 1:%d 2:%d\r", m1, m2);
+    }
+    
+    //clear robot msgs
+    while (rob.readable())
+        rob.getc();
+}