Diff: main.cpp

Revision:

12:90b5d8eae5ec

Parent:

11:0309bef74ba8

Child:

13:ab52f46c98ab

--- a/main.cpp	Wed Feb 15 14:04:02 2017 -0600
+++ b/main.cpp	Tue Mar 27 14:35:09 2018 +0000
@@ -1,9 +1,80 @@
-#include "mbed.h"
-#include "rtos.h"
- 
-DigitalOut led1(LED1);
+#include "core.h"
+
+DigitalOut led1(LED3);
 DigitalOut led2(LED2);
-Thread thread;
+Thread thread,thread_group_2HZ,thread_group_1HZ;
+
+//Merge tasks with same frequency
+namespace runTimeParams{
+    Mutex liveAccess;
+    float brakeForce = 0.0f;
+    float accelForce = 0.0f;
+    float avgSpeed = 0.0f;
+    float odometer = 0.0f;
+    #if DEBUG_MODE
+    string debugLog = "task,execution_time_ms,lastSleep,drift\n";
+    #endif
+    }
+
+
+namespace task_group_1{
+    Thread thread;
+    Timer executionTimer,sleepTimer;
+    bool tick = false;
+    
+    void init(){
+        executionTimer.reset();
+        sleepTimer.reset();
+        display::init();
+    }
+    void runTask(){
+        const int const_delay = int(1000.0f/display::freq);
+        int dynamic_delay = const_delay;
+        while(true){
+            sleepTimer.stop();
+            executionTimer.start();
+            
+            
+            int sleepTime = sleepTimer.read_ms();
+            const int drift = ((sleepTime - dynamic_delay) > 0)?
+                                        (sleepTime - dynamic_delay) : 0;
+            
+            
+            // Run all tasks
+            display::hotLoop();
+            
+            
+            executionTimer.stop();
+            int exec_time = executionTimer.read_ms();
+            
+            #if DEBUG_MODE
+            runTimeParams::liveAccess.lock();
+            //runTimeParams::debugLog += "GROUP_1," + to_string(executionTimer.read_ms()) + ","
+//                        + to_string(sleepTimer.read_ms()) + ", \n";
+            runTimeParams::odometer = float(sleepTime);
+            runTimeParams::avgSpeed = float(exec_time);
+            runTimeParams::liveAccess.unlock();
+            #endif
+            
+            executionTimer.reset();
+            sleepTimer.reset();
+            sleepTimer.start();
+            dynamic_delay = const_delay - (exec_time + drift);
+            Thread::wait(dynamic_delay);
+        }   
+    }
+}
+
+int init(){
+    //Run task initializers
+    task_group_1::init();
+    
+    //Start task hotloops
+    
+    task_group_1::thread.start(task_group_1::runTask);
+//    getControls::thread.start(getControls::runTask);
+    return 0;
+}
  
 void led2_thread() {
     while (true) {
@@ -13,6 +84,7 @@
 }
  
 int main() {
+    init();
     thread.start(led2_thread);
     
     while (true) {