GIU\ZF
Dependencies: MCP23017 WattBob_TextLCD mbed-rtos mbed
Fork of rtos_basic by
tasks/task_group2.cpp@20:202e0046527e, 2018-03-28 (annotated)
- Committer:
- ihexx
- Date:
- Wed Mar 28 14:34:34 2018 +0000
- Revision:
- 20:202e0046527e
- Parent:
- 19:2044bb5d7f29
LAST MINUTE CHJECKS
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ihexx | 15:524de2b2ef8e | 1 | #include "core.h" |
ihexx | 15:524de2b2ef8e | 2 | namespace getControls{ |
ihexx | 19:2044bb5d7f29 | 3 | //Read Accelerator and Brake |
ihexx | 15:524de2b2ef8e | 4 | AnalogIn brake(PORT_BRAKE); |
ihexx | 15:524de2b2ef8e | 5 | AnalogIn accel(PORT_ACCEL); |
ihexx | 15:524de2b2ef8e | 6 | const float freq = 10.0f; |
ihexx | 15:524de2b2ef8e | 7 | |
ihexx | 15:524de2b2ef8e | 8 | static inline void hotLoop(){ |
ihexx | 15:524de2b2ef8e | 9 | runTimeParams::liveAccess.lock(); |
ihexx | 15:524de2b2ef8e | 10 | runTimeParams::brakeForce = brake.read(); |
ihexx | 20:202e0046527e | 11 | runTimeParams::accelForce = accel.read(); |
ihexx | 15:524de2b2ef8e | 12 | runTimeParams::liveAccess.unlock(); |
ihexx | 15:524de2b2ef8e | 13 | } |
ihexx | 15:524de2b2ef8e | 14 | } |
ihexx | 16:0ada6cbd41e2 | 15 | namespace getIgnition{ |
ihexx | 16:0ada6cbd41e2 | 16 | //Read engine on/off switch and show current state on a LED |
ihexx | 19:2044bb5d7f29 | 17 | const float freq = 2; //hz |
ihexx | 16:0ada6cbd41e2 | 18 | DigitalIn ignition(PORT_IGNITION); |
ihexx | 16:0ada6cbd41e2 | 19 | DigitalOut led1(IGNITION_LED); |
ihexx | 16:0ada6cbd41e2 | 20 | |
ihexx | 16:0ada6cbd41e2 | 21 | static inline void hotLoop(){ |
ihexx | 16:0ada6cbd41e2 | 22 | led1 = ignition.read(); |
ihexx | 16:0ada6cbd41e2 | 23 | } |
ihexx | 16:0ada6cbd41e2 | 24 | } |
ihexx | 15:524de2b2ef8e | 25 | namespace carSimulator{ |
ihexx | 19:2044bb5d7f29 | 26 | //Compute speed given car controls and timing data |
ihexx | 15:524de2b2ef8e | 27 | const float freq = 20.0f; |
ihexx | 15:524de2b2ef8e | 28 | static inline void hotLoop(){ |
ihexx | 19:2044bb5d7f29 | 29 | const float friction = 0.1f; //constant retardation |
ihexx | 17:a29ce6fc667c | 30 | runTimeParams::liveAccess.lock(); |
ihexx | 16:0ada6cbd41e2 | 31 | |
ihexx | 19:2044bb5d7f29 | 32 | //Check if ignition is on, and don't accelerate if it isn't |
ihexx | 17:a29ce6fc667c | 33 | float accel; |
ihexx | 17:a29ce6fc667c | 34 | if (getIgnition::ignition.read()){ |
ihexx | 17:a29ce6fc667c | 35 | accel = runTimeParams::accelForce - |
ihexx | 17:a29ce6fc667c | 36 | (runTimeParams::brakeForce+friction); |
ihexx | 17:a29ce6fc667c | 37 | } |
ihexx | 17:a29ce6fc667c | 38 | else{ |
ihexx | 17:a29ce6fc667c | 39 | accel= -(runTimeParams::brakeForce+friction); |
ihexx | 17:a29ce6fc667c | 40 | } |
ihexx | 17:a29ce6fc667c | 41 | |
ihexx | 19:2044bb5d7f29 | 42 | static int i = 0; //iterator for speed |
ihexx | 19:2044bb5d7f29 | 43 | const int i_prev = i; |
ihexx | 19:2044bb5d7f29 | 44 | i = (i>=3)? 0: i+1; |
ihexx | 19:2044bb5d7f29 | 45 | //Store to speed array in round robin format |
ihexx | 20:202e0046527e | 46 | float tmpSpeed = accel + runTimeParams::speed[i_prev]; |
ihexx | 20:202e0046527e | 47 | tmpSpeed = (tmpSpeed>0)?tmpSpeed:0; |
ihexx | 20:202e0046527e | 48 | tmpSpeed = (tmpSpeed>200)?200:tmpSpeed; |
ihexx | 20:202e0046527e | 49 | runTimeParams::speed[i] = tmpSpeed; |
ihexx | 20:202e0046527e | 50 | runTimeParams::odometer += tmpSpeed*0.01f; |
ihexx | 15:524de2b2ef8e | 51 | runTimeParams::liveAccess.unlock(); |
ihexx | 15:524de2b2ef8e | 52 | } |
ihexx | 15:524de2b2ef8e | 53 | } |
ihexx | 16:0ada6cbd41e2 | 54 | namespace filterSpeed{ |
ihexx | 17:a29ce6fc667c | 55 | //Filter speed with averaging filter |
ihexx | 16:0ada6cbd41e2 | 56 | static const float freq = 5; //hz |
ihexx | 17:a29ce6fc667c | 57 | static inline void hotLoop(){ |
ihexx | 16:0ada6cbd41e2 | 58 | runTimeParams::liveAccess.lock(); |
ihexx | 17:a29ce6fc667c | 59 | runTimeParams::avgSpeed = (runTimeParams::speed[0] + |
ihexx | 17:a29ce6fc667c | 60 | runTimeParams::speed[1] + |
ihexx | 17:a29ce6fc667c | 61 | runTimeParams::speed[2])/3; |
ihexx | 16:0ada6cbd41e2 | 62 | runTimeParams::liveAccess.unlock(); |
ihexx | 17:a29ce6fc667c | 63 | } |
ihexx | 16:0ada6cbd41e2 | 64 | } |
ihexx | 15:524de2b2ef8e | 65 | |
ihexx | 17:a29ce6fc667c | 66 | |
ihexx | 15:524de2b2ef8e | 67 | namespace task_group_2{ |
ihexx | 15:524de2b2ef8e | 68 | Thread thread; |
ihexx | 15:524de2b2ef8e | 69 | const float freq = 20.0f; //hz |
ihexx | 15:524de2b2ef8e | 70 | |
ihexx | 15:524de2b2ef8e | 71 | void runTask(){ |
ihexx | 19:2044bb5d7f29 | 72 | //Init |
ihexx | 15:524de2b2ef8e | 73 | Timer executionTimer,sleepTimer; |
ihexx | 15:524de2b2ef8e | 74 | executionTimer.reset(); |
ihexx | 15:524de2b2ef8e | 75 | sleepTimer.reset(); |
ihexx | 15:524de2b2ef8e | 76 | |
ihexx | 19:2044bb5d7f29 | 77 | const int const_delay = int((1000.0f/freq)+0.5f); //ideal scheduling rate |
ihexx | 19:2044bb5d7f29 | 78 | int dynamic_delay = const_delay; //compensating for release/execution time |
ihexx | 19:2044bb5d7f29 | 79 | int tick = 0; //downsample task frequencies |
ihexx | 19:2044bb5d7f29 | 80 | |
ihexx | 19:2044bb5d7f29 | 81 | #if DEBUG_MODE |
ihexx | 19:2044bb5d7f29 | 82 | int max_exec_time = 0; //for logging |
ihexx | 19:2044bb5d7f29 | 83 | #endif |
ihexx | 19:2044bb5d7f29 | 84 | |
ihexx | 15:524de2b2ef8e | 85 | while(true){ |
ihexx | 19:2044bb5d7f29 | 86 | //Determine scheduling compensators: |
ihexx | 19:2044bb5d7f29 | 87 | //1: release time drift |
ihexx | 19:2044bb5d7f29 | 88 | //2: execution time |
ihexx | 16:0ada6cbd41e2 | 89 | sleepTimer.stop(); |
ihexx | 16:0ada6cbd41e2 | 90 | executionTimer.start(); |
ihexx | 16:0ada6cbd41e2 | 91 | int sleepTime = sleepTimer.read_ms(); |
ihexx | 16:0ada6cbd41e2 | 92 | const int drift = ((sleepTime - dynamic_delay) > 0)? |
ihexx | 16:0ada6cbd41e2 | 93 | (sleepTime - dynamic_delay) : 0; |
ihexx | 15:524de2b2ef8e | 94 | |
ihexx | 16:0ada6cbd41e2 | 95 | |
ihexx | 16:0ada6cbd41e2 | 96 | // Run all tasks-------------- |
ihexx | 16:0ada6cbd41e2 | 97 | carSimulator::hotLoop(); |
ihexx | 16:0ada6cbd41e2 | 98 | |
ihexx | 17:a29ce6fc667c | 99 | static const int tick_interval_filter = int((freq/filterSpeed::freq)+0.5f); |
ihexx | 17:a29ce6fc667c | 100 | if (!(tick%tick_interval_filter)) filterSpeed::hotLoop(); |
ihexx | 17:a29ce6fc667c | 101 | |
ihexx | 16:0ada6cbd41e2 | 102 | static const int tick_interval_controls = int((freq/getControls::freq)+0.5f); |
ihexx | 16:0ada6cbd41e2 | 103 | if (!(tick%tick_interval_controls)) getControls::hotLoop(); |
ihexx | 16:0ada6cbd41e2 | 104 | |
ihexx | 16:0ada6cbd41e2 | 105 | static const int tick_interval_ignitionLED = int((freq/getIgnition::freq)+0.5f); |
ihexx | 16:0ada6cbd41e2 | 106 | if (!(tick%tick_interval_ignitionLED )) getIgnition::hotLoop(); |
ihexx | 19:2044bb5d7f29 | 107 | //-------------Completed tasks |
ihexx | 15:524de2b2ef8e | 108 | |
ihexx | 16:0ada6cbd41e2 | 109 | tick++; |
ihexx | 16:0ada6cbd41e2 | 110 | executionTimer.stop(); |
ihexx | 16:0ada6cbd41e2 | 111 | int exec_time = executionTimer.read_ms(); |
ihexx | 19:2044bb5d7f29 | 112 | |
ihexx | 15:524de2b2ef8e | 113 | |
ihexx | 15:524de2b2ef8e | 114 | #if DEBUG_MODE |
ihexx | 19:2044bb5d7f29 | 115 | //Debug Logs (once per dequeue call to avoid memory issues) |
ihexx | 19:2044bb5d7f29 | 116 | if (exec_time > max_exec_time) max_exec_time=exec_time; |
ihexx | 17:a29ce6fc667c | 117 | static const int debug_log_interval = int(freq/dequeueMail::freq); |
ihexx | 16:0ada6cbd41e2 | 118 | if (!(tick%debug_log_interval)){ |
ihexx | 17:a29ce6fc667c | 119 | runTimeParams::debugAccess.lock(); |
ihexx | 17:a29ce6fc667c | 120 | *runTimeParams::debugLog += "task_group_2," + to_string(max_exec_time) + "," |
ihexx | 16:0ada6cbd41e2 | 121 | + to_string(sleepTime) + "," |
ihexx | 16:0ada6cbd41e2 | 122 | + to_string(drift) + "\n\r"; |
ihexx | 17:a29ce6fc667c | 123 | exec_time = 0; |
ihexx | 17:a29ce6fc667c | 124 | runTimeParams::debugAccess.unlock(); |
ihexx | 16:0ada6cbd41e2 | 125 | } |
ihexx | 17:a29ce6fc667c | 126 | #else |
ihexx | 17:a29ce6fc667c | 127 | static const int debug_log_interval = 1; |
ihexx | 17:a29ce6fc667c | 128 | #endif |
ihexx | 17:a29ce6fc667c | 129 | |
ihexx | 19:2044bb5d7f29 | 130 | //Reset tick count |
ihexx | 17:a29ce6fc667c | 131 | static const int tick_LCM = |
ihexx | 17:a29ce6fc667c | 132 | debug_log_interval* |
ihexx | 17:a29ce6fc667c | 133 | tick_interval_ignitionLED* |
ihexx | 17:a29ce6fc667c | 134 | tick_interval_controls* |
ihexx | 17:a29ce6fc667c | 135 | tick_interval_filter; |
ihexx | 17:a29ce6fc667c | 136 | |
ihexx | 16:0ada6cbd41e2 | 137 | if (tick==tick_LCM) tick=0; |
ihexx | 15:524de2b2ef8e | 138 | |
ihexx | 16:0ada6cbd41e2 | 139 | executionTimer.reset(); |
ihexx | 16:0ada6cbd41e2 | 140 | sleepTimer.reset(); |
ihexx | 16:0ada6cbd41e2 | 141 | sleepTimer.start(); |
ihexx | 19:2044bb5d7f29 | 142 | //compensate for delays |
ihexx | 17:a29ce6fc667c | 143 | dynamic_delay = const_delay -(exec_time + drift); |
ihexx | 15:524de2b2ef8e | 144 | Thread::wait(dynamic_delay); |
ihexx | 15:524de2b2ef8e | 145 | } |
ihexx | 15:524de2b2ef8e | 146 | } |
ihexx | 15:524de2b2ef8e | 147 | } |