From c7b88455e576a49a0d015fcf1681f2a4e1eec421 Mon Sep 17 00:00:00 2001
From: Eugene Batalov <eabatalov89@gmail.com>
Date: Thu, 7 Jul 2016 23:27:07 +0300
Subject: [PATCH] car_fmw: implement 5 demo car programs

Demo programs:
1. Go forward
2. Go backward
3. Rotate right
4. Rotate left
5. Complicated route
All programs are running in a cycle

+ Refactor delay.h code to have common code style with main.c
---
 car_fmw/src/delay.h | 16 ---------
 car_fmw/src/main.c  | 87 +++++++++++++++++++++++++++++++--------------
 car_fmw/src/wait.h  |  6 ++++
 3 files changed, 67 insertions(+), 42 deletions(-)
 delete mode 100644 car_fmw/src/delay.h
 create mode 100644 car_fmw/src/wait.h

diff --git a/car_fmw/src/delay.h b/car_fmw/src/delay.h
deleted file mode 100644
index 492981a32aa..00000000000
--- a/car_fmw/src/delay.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#pragma once
-
-#include "stm32f4_discovery.h"
-#include "stm32f4xx_conf.h"
-
-/**
-  * @brief  Delay Function.
-  * @param  nCount:specifies the Delay time length.
-  * @retval None
-  */
-inline void Delay(__IO uint32_t nCount)
-{
-  while(nCount--)
-  {
-  }
-}
diff --git a/car_fmw/src/main.c b/car_fmw/src/main.c
index 198cfdff03a..a7bcf4f39df 100644
--- a/car_fmw/src/main.c
+++ b/car_fmw/src/main.c
@@ -1,10 +1,67 @@
 #include "car_leds.h"
 #include "car_engine.h"
-#include "delay.h"
+#include "wait.h"
+
+const uint32_t PROGRAM_DURATION = 0x3FFFFF; 
+static void program_forward(void)
+{
+    engine_forward();
+    wait(PROGRAM_DURATION);
+}
+
+static void program_backward(void)
+{
+    engine_backward();
+    wait(PROGRAM_DURATION);
+}
+
+static void program_rotation_left(void)
+{
+    engine_turn_left();
+    wait(PROGRAM_DURATION);
+}
+
+static void program_rotation_right(void)
+{
+    engine_turn_right();
+    wait(PROGRAM_DURATION);
+}
+
+static void program_arbitraty_route(void)
+{
+    const int ROUTE_PIECE_DURATION = 6 * PROGRAM_DURATION;
+    engine_forward();
+    wait(ROUTE_PIECE_DURATION);
+
+    engine_backward();
+    wait(ROUTE_PIECE_DURATION);
+
+    engine_forward();
+    wait(ROUTE_PIECE_DURATION / 2);
+
+    engine_turn_right();
+    wait(ROUTE_PIECE_DURATION);
+
+    engine_forward();
+    wait(ROUTE_PIECE_DURATION / 2);
+
+    engine_backward();
+    wait(ROUTE_PIECE_DURATION / 2);
+
+    engine_turn_left();
+    wait(ROUTE_PIECE_DURATION);
+
+    engine_backward();
+    wait(ROUTE_PIECE_DURATION / 2);
+
+    engine_stop();
+    wait(ROUTE_PIECE_DURATION / 2);
+}
+
 
 int main(void)
 {
-    /*!< At this stage the microcontroller clock setting is already configured, 
+    /*!< At this stage the microcontroller clock setting is already configured,
        this is done through SystemInit() function which is called from startup
        file (startup_stm32f4xx.s) before to branch to application main.
        To reconfigure the default setting of SystemInit() function, refer to
@@ -14,29 +71,7 @@ int main(void)
     leds_init();
     engine_init();
 
-    const int DELAY = 0x3FFFFF;
-    while (1)
-    {
-        led_set(LED_GREEN, true);
-        Delay(DELAY);
-
-        engine_forward();
-        led_set(LED_ORANGE, true);
-        Delay(DELAY);
-
-        engine_stop();
-        led_set(LED_RED, true);
-        Delay(DELAY);
-
-        engine_backward();
-        led_set(LED_BLUE, true);
-        Delay(DELAY);
-
-        engine_stop();
-        led_set(LED_GREEN, false);
-        led_set(LED_ORANGE, false);
-        led_set(LED_RED, false);
-        led_set(LED_BLUE, false);
-        Delay(DELAY);
+    while(1) {
+        program_forward();
     }
 }
diff --git a/car_fmw/src/wait.h b/car_fmw/src/wait.h
new file mode 100644
index 00000000000..227b37c9775
--- /dev/null
+++ b/car_fmw/src/wait.h
@@ -0,0 +1,6 @@
+#pragma once
+
+inline void wait(uint32_t loops)
+{
+    while(--loops);
+}