Diff: main.cpp

Revision:

8:5c0833506d67

Parent:

7:9068fc754a0b

Child:

9:5292bf545459

--- a/main.cpp	Thu Dec 03 17:53:04 2015 +0000
+++ b/main.cpp	Fri Dec 11 10:40:40 2015 +0000
@@ -3,12 +3,16 @@
 #include "M3Dpi.h"
 #include "jsonReporter.h"
 
-const char M3DPI_ID[] = "1234567890";
+const char M3DPI_ID[] = "first";
 
 M3Dpi robot;
 Serial pc(USBTX,USBRX);
 JsonReporter report(&pc, M3DPI_ID);
 
+static float r_wheel = 0.0;
+static float l_wheel = 0.0;
+char direction = 'f';
+
 
 void setLeds(m3dpi::Distance distance)
 {
@@ -32,19 +36,66 @@
     report.acceleration(robot.getAcceleration());
     
     report.time(robot.getTime());
-    robot.setStatus(Color::OFF);
+    robot.setStatus(Color::BLACK);
+}
+
+void read_commands_thread(void const *args)
+{
+    if(pc.readable()){
+        char command = pc.getc();
+        switch(command){
+            case 'l':
+                // Left
+                if(direction == 'f'){
+                    l_wheel += 0.1;
+                }else{
+                    l_wheel -= 0.1;
+                }
+                break;
+            case 'r':
+                // Right
+                if(direction == 'f'){
+                    r_wheel += 0.1;
+                }else{
+                    r_wheel -= 0.1;
+                }
+                break;
+            case 'f':
+                // Frontward
+                r_wheel += 0.1;
+                l_wheel += 0.1;
+                direction = 'f';
+                break;
+            case 'b':
+                // Backward
+                r_wheel -= 0.1;
+                l_wheel -= 0.1;
+                direction = 'b';
+                break;
+            case 's':
+                // Brake
+                r_wheel = 0;
+                l_wheel = 0;
+                direction = 'f';
+                break;
+            default:
+                pc.printf("Unknown command %c.\n", command);
+        }
+        
+        robot.left_motor(l_wheel);
+        robot.right_motor(r_wheel);
+    }
 }
 
 int main()
 {            
     pc.baud(115200);
-       
-    // for testing...
-    pc.printf("Device ID is: 0x%02x\n", robot.accelerometer.getDevId());
-    pc.printf("Gyro id: 0x%02x\n", robot.gyro.getWhoAmI());
     
-    RtosTimer periodicThread(read_sensors_thread, osTimerPeriodic);
-    periodicThread.start(50);
+    RtosTimer readSensorsThread(read_sensors_thread, osTimerPeriodic);
+    readSensorsThread.start(1000);
+
+    RtosTimer readCommandsThread(read_commands_thread, osTimerPeriodic);
+    readCommandsThread.start(50);
     
     Thread::wait(osWaitForever);
 }
\ No newline at end of file