GIU\ZF
Dependencies: MCP23017 WattBob_TextLCD mbed-rtos mbed
Fork of rtos_basic by
Diff: tasks/task_group2.cpp
- Revision:
- 19:2044bb5d7f29
- Parent:
- 17:a29ce6fc667c
- Child:
- 20:202e0046527e
diff -r d48324fd3440 -r 2044bb5d7f29 tasks/task_group2.cpp --- a/tasks/task_group2.cpp Wed Mar 28 00:47:12 2018 +0000 +++ b/tasks/task_group2.cpp Wed Mar 28 01:32:01 2018 +0000 @@ -1,9 +1,8 @@ #include "core.h" namespace getControls{ - //I/O + //Read Accelerator and Brake AnalogIn brake(PORT_BRAKE); AnalogIn accel(PORT_ACCEL); - const float freq = 10.0f; static inline void hotLoop(){ @@ -15,9 +14,7 @@ } namespace getIgnition{ //Read engine on/off switch and show current state on a LED - static const float freq = 2; //hz - - //I/O + const float freq = 2; //hz DigitalIn ignition(PORT_IGNITION); DigitalOut led1(IGNITION_LED); @@ -25,22 +22,14 @@ led1 = ignition.read(); } } - namespace carSimulator{ - Thread thread; + //Compute speed given car controls and timing data const float freq = 20.0f; - static inline void hotLoop(){ - - static int i = 0; - const int i_prev = i; - i = (i>=3)? 0: i+1; - - const float friction = 0.1f; - + const float friction = 0.1f; //constant retardation runTimeParams::liveAccess.lock(); - //v2 = at+v1 + //Check if ignition is on, and don't accelerate if it isn't float accel; if (getIgnition::ignition.read()){ accel = runTimeParams::accelForce - @@ -50,10 +39,14 @@ accel= -(runTimeParams::brakeForce+friction); } + static int i = 0; //iterator for speed + const int i_prev = i; + i = (i>=3)? 0: i+1; + //Store to speed array in round robin format float tmpSpeed = accel * + runTimeParams::speed[i_prev]; runTimeParams::speed[i] = (tmpSpeed>0)?tmpSpeed:0; + runTimeParams::liveAccess.unlock(); - } } namespace filterSpeed{ @@ -65,7 +58,6 @@ runTimeParams::speed[1] + runTimeParams::speed[2])/3; runTimeParams::liveAccess.unlock(); - } } @@ -73,24 +65,27 @@ namespace task_group_2{ Thread thread; const float freq = 20.0f; //hz - DigitalOut led2(LED2); void runTask(){ + //Init Timer executionTimer,sleepTimer; executionTimer.reset(); sleepTimer.reset(); - const int const_delay = int((1000.0f/freq)+0.5f); - int dynamic_delay = const_delay; - int tick = 0; - int max_exec_time = 0; + const int const_delay = int((1000.0f/freq)+0.5f); //ideal scheduling rate + int dynamic_delay = const_delay; //compensating for release/execution time + int tick = 0; //downsample task frequencies + + #if DEBUG_MODE + int max_exec_time = 0; //for logging + #endif + while(true){ - - + //Determine scheduling compensators: + //1: release time drift + //2: execution time sleepTimer.stop(); executionTimer.start(); - - int sleepTime = sleepTimer.read_ms(); const int drift = ((sleepTime - dynamic_delay) > 0)? (sleepTime - dynamic_delay) : 0; @@ -107,14 +102,16 @@ static const int tick_interval_ignitionLED = int((freq/getIgnition::freq)+0.5f); if (!(tick%tick_interval_ignitionLED )) getIgnition::hotLoop(); - // Completed tasks + //-------------Completed tasks tick++; executionTimer.stop(); int exec_time = executionTimer.read_ms(); - if (exec_time > max_exec_time) max_exec_time=exec_time; + #if DEBUG_MODE + //Debug Logs (once per dequeue call to avoid memory issues) + if (exec_time > max_exec_time) max_exec_time=exec_time; static const int debug_log_interval = int(freq/dequeueMail::freq); if (!(tick%debug_log_interval)){ runTimeParams::debugAccess.lock(); @@ -128,6 +125,7 @@ static const int debug_log_interval = 1; #endif + //Reset tick count static const int tick_LCM = debug_log_interval* tick_interval_ignitionLED* @@ -139,7 +137,7 @@ executionTimer.reset(); sleepTimer.reset(); sleepTimer.start(); - + //compensate for delays dynamic_delay = const_delay -(exec_time + drift); Thread::wait(dynamic_delay); }