Diff: main.cpp

Revision:

11:8b79f6545628

Parent:

9:46c42975a0dc

Parent:

10:f92664a22d02

Child:

12:221c9e02ea96

--- a/main.cpp	Thu Feb 04 17:52:23 2016 +0000
+++ b/main.cpp	Thu Feb 04 19:39:56 2016 +0000
@@ -14,6 +14,7 @@
 DigitalOut heartbeatLED(LED1);
 DigitalOut buttonLED(LED2);
 DigitalOut redlightLED(LED3);
+DigitalOut led4(LED4);
 
 DigitalIn buttonOne(p25);
 DigitalIn buttonTwo(p26);
@@ -54,17 +55,43 @@
 
 // Querregelung Ende
 
+void hearbeat_thread(void const *args);
+
 void serial_thread(void const *args) {
-    char c;
+    size_t length = 20;
+    char receive_buffer[length];
+    char* ptr = (char*) receive_buffer;
+    char* end = ptr + length;
+    char c, x = 22;
     float f;
+    int return_value = 666;
     while (true) {
-        serialMinnow.scanf("[%c]=%f\r", &c, &f);
-        if (c == 'v') {
-            serialMinnow.printf("v erhalten: %f", c);
-        } else if (c == 'g') {
-            serialMinnow.printf("g erhalten: %f", c);
+        if (ptr != end) {
+            c = serialMinnow.getc();
+            *ptr++ = c;
+        } else {
+            ptr = (char*)receive_buffer;
         }
         heartbeatLED = !heartbeatLED;
+        
+        if (c == '\r') return_value = sscanf(receive_buffer, "[%c]=%f\r", &x, &f);
+        
+        serialMinnow.printf("%d\r\n", return_value);
+        
+        if (x == 'v') {
+            serialMinnow.printf("v erhalten: %f\r\n", f);
+            buttonLED = true;
+            // Buffer wieder auf Anfang setzen
+            ptr = (char*)receive_buffer;
+            x = 22;
+        } else if (x == 'g') {
+            serialMinnow.printf("g erhalten: %f\r\n", f);
+            redlightLED = true;
+            // Buffer wieder auf Anfang setzen
+            ptr = (char*)receive_buffer;
+            x = 22;
+        }
+        
         //Thread::wait(100);
     }
 }
@@ -120,6 +147,7 @@
     quadratureController = new QuadratureController(&steerPWM, &quadrature_queue, &imu_queue_steering_angle);
 
     Thread machineDirectionControl(serial_thread);
+    Thread hearbeatThread(hearbeat_thread);
     
     RtosTimer machine_direction_control_timer(machine_direction_control);
     RtosTimer quadrature_control_timer(redirect_quadrature_controller);
@@ -170,8 +198,8 @@
         if (rc_percentage > (uint16_t) 1800 && rc_valid != 0) {
             // oben => Wettbewerb
             //heartbeatLED = true;
-            buttonLED = false;
-            redlightLED = false;
+            //buttonLED = false;
+            //redlightLED = false;
             supportSystem->setLightManagerRemoteLight(false, true);
             if (!timer_started) {
                 timer_started = true;
@@ -181,8 +209,8 @@
         } else if (rc_percentage > (uint16_t) 1200 && rc_valid != 0) {
             // unten => RC-Wettbewerb
             //heartbeatLED = false;
-            buttonLED = false;
-            redlightLED = true;
+            //buttonLED = false;
+            //redlightLED = true;
             supportSystem->setLightManagerRemoteLight(true, true);
             if (drive_valid) {
                 drivePWM.pulsewidth_us(drive_percentage);
@@ -198,8 +226,8 @@
         } else if (rc_percentage > (uint16_t) 800 && rc_valid != 0) {
             // mitte => RC-Training
             //heartbeatLED = false;
-            buttonLED = true;
-            redlightLED = false;
+            //buttonLED = true;
+            //redlightLED = false;
             supportSystem->setLightManagerRemoteLight(true, true);
             if (drive_valid) {
                 drivePWM.pulsewidth_us(drive_percentage);
@@ -234,3 +262,10 @@
         Thread::wait(50);
     }
 }
+
+void hearbeat_thread(void const *args) {
+    while(true) {
+        led4 = !led4;
+        Thread::wait(100);
+    }
+}