GIU\ZF
Dependencies: MCP23017 WattBob_TextLCD mbed-rtos mbed
Fork of rtos_basic by
tasks/task_group2.cpp
- Committer:
- ihexx
- Date:
- 2018-03-28
- Revision:
- 19:2044bb5d7f29
- Parent:
- 17:a29ce6fc667c
- Child:
- 20:202e0046527e
File content as of revision 19:2044bb5d7f29:
#include "core.h" namespace getControls{ //Read Accelerator and Brake AnalogIn brake(PORT_BRAKE); AnalogIn accel(PORT_ACCEL); const float freq = 10.0f; static inline void hotLoop(){ runTimeParams::liveAccess.lock(); runTimeParams::brakeForce = brake.read(); runTimeParams::accelForce = brake.read(); runTimeParams::liveAccess.unlock(); } } namespace getIgnition{ //Read engine on/off switch and show current state on a LED const float freq = 2; //hz DigitalIn ignition(PORT_IGNITION); DigitalOut led1(IGNITION_LED); static inline void hotLoop(){ led1 = ignition.read(); } } namespace carSimulator{ //Compute speed given car controls and timing data const float freq = 20.0f; static inline void hotLoop(){ const float friction = 0.1f; //constant retardation runTimeParams::liveAccess.lock(); //Check if ignition is on, and don't accelerate if it isn't float accel; if (getIgnition::ignition.read()){ accel = runTimeParams::accelForce - (runTimeParams::brakeForce+friction); } else{ 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{ //Filter speed with averaging filter static const float freq = 5; //hz static inline void hotLoop(){ runTimeParams::liveAccess.lock(); runTimeParams::avgSpeed = (runTimeParams::speed[0] + runTimeParams::speed[1] + runTimeParams::speed[2])/3; runTimeParams::liveAccess.unlock(); } } namespace task_group_2{ Thread thread; const float freq = 20.0f; //hz void runTask(){ //Init Timer executionTimer,sleepTimer; executionTimer.reset(); sleepTimer.reset(); 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; // Run all tasks-------------- carSimulator::hotLoop(); static const int tick_interval_filter = int((freq/filterSpeed::freq)+0.5f); if (!(tick%tick_interval_filter)) filterSpeed::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(); #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(); *runTimeParams::debugLog += "task_group_2," + to_string(max_exec_time) + "," + to_string(sleepTime) + "," + to_string(drift) + "\n\r"; exec_time = 0; runTimeParams::debugAccess.unlock(); } #else static const int debug_log_interval = 1; #endif //Reset tick count static const int tick_LCM = debug_log_interval* tick_interval_ignitionLED* tick_interval_controls* tick_interval_filter; if (tick==tick_LCM) tick=0; executionTimer.reset(); sleepTimer.reset(); sleepTimer.start(); //compensate for delays dynamic_delay = const_delay -(exec_time + drift); Thread::wait(dynamic_delay); } } }