From 9f374543937468a4c902c99024eefc861b0e7737 Mon Sep 17 00:00:00 2001
From: Eugene Batalov <eabatalov89@gmail.com>
Date: Wed, 13 Jul 2016 23:32:06 +0300
Subject: [PATCH] car_fmw: implement remote control car mode

---
 car_fmw/src/car_rc.c | 50 +++++++++++++++++++++++++++++++++++++++++++-
 car_fmw/src/time.c   |  2 ++
 2 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/car_fmw/src/car_rc.c b/car_fmw/src/car_rc.c
index 48030b65165..c15f468931b 100644
--- a/car_fmw/src/car_rc.c
+++ b/car_fmw/src/car_rc.c
@@ -6,6 +6,22 @@
 #include "car_leds.h"
 #include "car_engine.h"
 
+typedef enum {
+    RC_CAR_CMD_STOP = '0',
+    RC_CAR_CMD_FWD = '1',
+    RC_CAR_CMD_BKWD = '2',
+    RC_CAR_CMD_RIGHT = '3',
+    RC_CAR_CMD_LEFT = '4'
+} RC_CAR_CMD;
+
+void send_cmd_result(uint8_t cmd, uint8_t result)
+{
+    VCP_put_char('(');
+    VCP_put_char(cmd);
+    VCP_put_char(':');
+    VCP_put_char(result);
+    VCP_put_char(')');
+}
 
 void run_rc_car(volatile bool *stop)
 {
@@ -13,9 +29,41 @@ void run_rc_car(volatile bool *stop)
     led_set(LED_ORANGE, true);
     led_set(LED_RED, true);
     led_set(LED_BLUE, true);
+    engine_stop();
 
-    while(!*stop) {
+    // Cleanup VCP buffer. This doesn't clean all the garbage.
+    // Some garbage arrives much later then we read here.
+    uint8_t tmp_char;
+    while(VCP_get_char(&tmp_char));
 
+    uint8_t cur_cmd = RC_CAR_CMD_STOP;
+    uint8_t new_cmd = cur_cmd;
+
+    while(true) {
+        while((!*stop) && !VCP_get_char(&new_cmd)) {}
+        if (*stop)
+            break;
+
+        if (cur_cmd == new_cmd) {}
+        else if (new_cmd == RC_CAR_CMD_STOP)
+            engine_stop();
+        else if (new_cmd == RC_CAR_CMD_FWD)
+            engine_forward();
+        else if (new_cmd == RC_CAR_CMD_BKWD)
+            engine_backward();
+        else if (new_cmd == RC_CAR_CMD_RIGHT)
+            engine_turn_right();
+        else if (new_cmd == RC_CAR_CMD_LEFT)
+            engine_turn_left();
+        else {
+            send_cmd_result(new_cmd, '1');
+            continue;
+        }
+
+        send_cmd_result(new_cmd, '0');
+        cur_cmd = new_cmd;
     }
+
     leds_clear_all();
+    engine_stop();
 }
diff --git a/car_fmw/src/time.c b/car_fmw/src/time.c
index 8afba9e027a..d231768fe37 100644
--- a/car_fmw/src/time.c
+++ b/car_fmw/src/time.c
@@ -5,6 +5,8 @@ static volatile uint32_t pending_timer_ticks, ticks_since_boot;
 
 void time_init(void)
 {
+    // XXX this is copy pasted and it doesn't provide 1ms periodic
+    // timer interrupt
     if (SysTick_Config(SystemCoreClock / 1000))
         while (1){};
 }