Diff: tasks/task_group2.cpp

Revision:

16:0ada6cbd41e2

Parent:

15:524de2b2ef8e

Child:

17:a29ce6fc667c

--- a/tasks/task_group2.cpp	Tue Mar 27 18:55:43 2018 +0000
+++ b/tasks/task_group2.cpp	Tue Mar 27 22:03:07 2018 +0000
@@ -13,27 +13,66 @@
         runTimeParams::liveAccess.unlock();
     }
 }
+namespace getIgnition{
+    //Read engine on/off switch and show current state on a LED
+    static const float freq = 2; //hz
+    
+    //I/O
+    DigitalIn ignition(PORT_IGNITION);
+    DigitalOut led1(IGNITION_LED);
+    
+    static inline void hotLoop(){
+            led1 = ignition.read();
+    }
+}
 
 namespace carSimulator{
     Thread thread;
     const float freq = 20.0f;
     
     static inline void hotLoop(){
+        
         runTimeParams::liveAccess.lock();
         //a = (v2-v1)/t
         //v2 = at+v1
-        float tmpSpeed = runTimeParams::brakeForce - 
-                         runTimeParams::accelForce +
+        const float friction = 0.1f;
+        const float accel = (getIgnition::ignition.read())? (runTimeParams::accelForce - 
+                        (runTimeParams::brakeForce+friction))
+                        : 
+                        -(runTimeParams::brakeForce+friction);
+        
+        float tmpSpeed = accel * +
                          runTimeParams::newSpeed;
         runTimeParams::newSpeed = (tmpSpeed>0)?tmpSpeed:0;
         runTimeParams::liveAccess.unlock();
         }
 }
+namespace filterSpeed{
+    static const float freq = 5; //hz 
+    
+    //I/O
+    float speed[3] = {0};
+    
+    void runTask(){
+        //Filter speed with averaging filter
+        //• average of last ‘n’ readings (e.g. n =3)
+        //(raw speed value will be computed by the “car simulator”
+        //process)
+        while(1){
+            wait(1/freq);
+            float avgSpeed = (speed[0] + speed[1] +speed[2])/3;
+            runTimeParams::liveAccess.lock();
+            runTimeParams::avgSpeed = avgSpeed;
+            runTimeParams::liveAccess.unlock();
+            
+        } 
+    }
+}
 
 namespace task_group_2{
     Thread thread;
     const float freq = 20.0f; //hz
-    
+    DigitalOut led2(LED2);
 
     void runTask(){
         Timer executionTimer,sleepTimer;
@@ -43,48 +82,52 @@
         const int const_delay = int((1000.0f/freq)+0.5f);
         int dynamic_delay = const_delay;
         int tick = 0;
+        
         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--------------
-//            
-//            static const int tick_interval_controls = int((freq/getControls::freq)+0.5f);
-//            if (!(tick%tick_interval_controls ))
-//            getControls::hotLoop();
+            
+            
+            sleepTimer.stop();
+            executionTimer.start();
+            
+            
+            int sleepTime = sleepTimer.read_ms();
+            const int drift = ((sleepTime - dynamic_delay) > 0)?
+                                        (sleepTime - dynamic_delay) : 0;
             
-//            carSimulator::hotLoop();
+            
+            // Run all tasks--------------
+            carSimulator::hotLoop();
+            
+            static const int tick_interval_controls = int((freq/getControls::freq)+0.5f);
+            if (!(tick%tick_interval_controls)) getControls::hotLoop();
+            
+            static const int tick_interval_ignitionLED = int((freq/getIgnition::freq)+0.5f);
+            if (!(tick%tick_interval_ignitionLED )) getIgnition::hotLoop();
             // Completed tasks
             
-//            tick++;
-//            executionTimer.stop();
-//            int exec_time = executionTimer.read_ms();
+            tick++;
+            executionTimer.stop();
+            int exec_time = executionTimer.read_ms();
             
             #if DEBUG_MODE
-            //runTimeParams::liveAccess.lock();
-//            const int debug_log_interval = int((freq/dequeueMail::freq)+0.5f);
-//            if (!(tick%debug_log_interval)){
-//                runTimeParams::liveAccess.lock();
-//                runTimeParams::debugLog += "task_group_2," + to_string(exec_time) + ","
-//                            + to_string(sleepTime) + ","
-//                            + to_string(drift) + "tick interval getControls: " + to_string(tick_interval_controls) +"\n\r";
-//                runTimeParams::liveAccess.unlock();
-//                
-//            }
-//            if (tick==debug_log_interval*1) tick=0;
-//            runTimeParams::liveAccess.unlock();
+            const int debug_log_interval = int(freq/dequeueMail::freq);
+            if (!(tick%debug_log_interval)){
+                runTimeParams::liveAccess.lock();
+                runTimeParams::debugLog += "task_group_2," + to_string(exec_time) + ","
+                            + to_string(sleepTime) + ","
+                            + to_string(drift) + "\n\r";
+                
+                runTimeParams::liveAccess.unlock();
+            }
+            static const int tick_LCM = (debug_log_interval*tick_interval_ignitionLED*tick_interval_controls);
+            if (tick==tick_LCM) tick=0;
             #endif
             
-//            executionTimer.reset();
-//            sleepTimer.reset();
-//            sleepTimer.start();
-//            dynamic_delay = const_delay - (exec_time + drift);
+            executionTimer.reset();
+            sleepTimer.reset();
+            sleepTimer.start();
+            
+            dynamic_delay = const_delay -drift;
             Thread::wait(dynamic_delay);
         }   
     }