GIU\ZF
Dependencies: MCP23017 WattBob_TextLCD mbed-rtos mbed
Fork of rtos_basic by
tasks/task_group1.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 | 13:ab52f46c98ab | 1 | #include "core.h" |
ihexx | 15:524de2b2ef8e | 2 | namespace display{ |
ihexx | 19:2044bb5d7f29 | 3 | //Display on MBED text display: odometer value, average speed |
ihexx | 15:524de2b2ef8e | 4 | const float freq = 2.0f; //hz |
ihexx | 15:524de2b2ef8e | 5 | MCP23017 *port; |
ihexx | 15:524de2b2ef8e | 6 | WattBob_TextLCD *lcd; |
ihexx | 15:524de2b2ef8e | 7 | |
ihexx | 15:524de2b2ef8e | 8 | void init(){ |
ihexx | 15:524de2b2ef8e | 9 | port = new MCP23017(p9, p10, 0x40); |
ihexx | 15:524de2b2ef8e | 10 | lcd = new WattBob_TextLCD(port); |
ihexx | 15:524de2b2ef8e | 11 | port->write_bit(1,BL_BIT); // LCD backlight on. |
ihexx | 15:524de2b2ef8e | 12 | lcd->cls(); |
ihexx | 15:524de2b2ef8e | 13 | } |
ihexx | 15:524de2b2ef8e | 14 | static inline void hotLoop(){ |
ihexx | 15:524de2b2ef8e | 15 | lcd->cls(); |
ihexx | 15:524de2b2ef8e | 16 | |
ihexx | 15:524de2b2ef8e | 17 | runTimeParams::liveAccess.lock(); |
ihexx | 15:524de2b2ef8e | 18 | float odometer = runTimeParams::odometer; |
ihexx | 15:524de2b2ef8e | 19 | float avgSpeed = runTimeParams::avgSpeed; |
ihexx | 15:524de2b2ef8e | 20 | runTimeParams::liveAccess.unlock(); |
ihexx | 15:524de2b2ef8e | 21 | |
ihexx | 15:524de2b2ef8e | 22 | lcd->locate(0,0); //located col, row. |
ihexx | 15:524de2b2ef8e | 23 | lcd->printf("Odo=%.2f",odometer); |
ihexx | 15:524de2b2ef8e | 24 | |
ihexx | 15:524de2b2ef8e | 25 | lcd->locate(1,0); //located col, row. |
ihexx | 15:524de2b2ef8e | 26 | lcd->printf("Speed=%.2f",avgSpeed); |
ihexx | 15:524de2b2ef8e | 27 | } |
ihexx | 15:524de2b2ef8e | 28 | } |
ihexx | 17:a29ce6fc667c | 29 | namespace brakeIndicator{ |
ihexx | 17:a29ce6fc667c | 30 | //Show use of the brake on a LED on the RedBox Unit |
ihexx | 17:a29ce6fc667c | 31 | const float freq = 2.0f; //hz |
ihexx | 20:202e0046527e | 32 | DigitalOut led2(PORT_REDBOX_LED1); |
ihexx | 17:a29ce6fc667c | 33 | static inline void init(){ |
ihexx | 20:202e0046527e | 34 | // led2.period_ms(50); |
ihexx | 17:a29ce6fc667c | 35 | } |
ihexx | 17:a29ce6fc667c | 36 | static inline void hotLoop(){ |
ihexx | 17:a29ce6fc667c | 37 | runTimeParams::liveAccess.lock(); |
ihexx | 20:202e0046527e | 38 | if (runTimeParams::brakeForce>0.3f){ |
ihexx | 20:202e0046527e | 39 | // led2.write(runTimeParams::brakeForce*100.0f); |
ihexx | 20:202e0046527e | 40 | led2 = 1; |
ihexx | 20:202e0046527e | 41 | } |
ihexx | 20:202e0046527e | 42 | else{ |
ihexx | 20:202e0046527e | 43 | led2 = 0; |
ihexx | 20:202e0046527e | 44 | } |
ihexx | 17:a29ce6fc667c | 45 | runTimeParams::liveAccess.unlock(); |
ihexx | 17:a29ce6fc667c | 46 | |
ihexx | 17:a29ce6fc667c | 47 | } |
ihexx | 17:a29ce6fc667c | 48 | } |
ihexx | 17:a29ce6fc667c | 49 | namespace speedIndicator{ |
ihexx | 17:a29ce6fc667c | 50 | //Monitor speed and if it goes over 88 mph switch on a LED on |
ihexx | 17:a29ce6fc667c | 51 | //the RedBox unit |
ihexx | 17:a29ce6fc667c | 52 | static const float freq = 1; //hz |
ihexx | 20:202e0046527e | 53 | DigitalOut led(PORT_REDBOX_LED2); |
ihexx | 17:a29ce6fc667c | 54 | static inline void init(){ |
ihexx | 17:a29ce6fc667c | 55 | led = 0; |
ihexx | 17:a29ce6fc667c | 56 | } |
ihexx | 17:a29ce6fc667c | 57 | static inline void hotLoop(){ |
ihexx | 17:a29ce6fc667c | 58 | runTimeParams::liveAccess.lock(); |
ihexx | 20:202e0046527e | 59 | if (runTimeParams::avgSpeed>88.0f){ |
ihexx | 20:202e0046527e | 60 | led = 1; |
ihexx | 20:202e0046527e | 61 | } |
ihexx | 20:202e0046527e | 62 | else{ |
ihexx | 20:202e0046527e | 63 | led = 0; |
ihexx | 20:202e0046527e | 64 | } |
ihexx | 17:a29ce6fc667c | 65 | runTimeParams::liveAccess.unlock(); |
ihexx | 17:a29ce6fc667c | 66 | |
ihexx | 17:a29ce6fc667c | 67 | } |
ihexx | 17:a29ce6fc667c | 68 | } |
ihexx | 17:a29ce6fc667c | 69 | namespace sideLights{ |
ihexx | 17:a29ce6fc667c | 70 | //Read a single side light switch and set side lights accordingly |
ihexx | 17:a29ce6fc667c | 71 | static const float freq = 1; //hz |
ihexx | 17:a29ce6fc667c | 72 | DigitalIn lightSwitch(PORT_BRAKE); |
ihexx | 17:a29ce6fc667c | 73 | DigitalOut led(PORT_SIDE_LIGHTS); |
ihexx | 17:a29ce6fc667c | 74 | static inline void hotLoop(){ |
ihexx | 17:a29ce6fc667c | 75 | led = lightSwitch; |
ihexx | 17:a29ce6fc667c | 76 | } |
ihexx | 17:a29ce6fc667c | 77 | |
ihexx | 17:a29ce6fc667c | 78 | } |
ihexx | 17:a29ce6fc667c | 79 | namespace turnSignal{ |
ihexx | 17:a29ce6fc667c | 80 | //Read the two turn indicator switches and flash appropriate |
ihexx | 19:2044bb5d7f29 | 81 | //indicator LEDs at a rate of 1Hz |
ihexx | 17:a29ce6fc667c | 82 | static const float freq = 0.5; //hz |
ihexx | 17:a29ce6fc667c | 83 | DigitalIn lSwitch(PORT_TURN_SIGNAL_SWITCH_LEFT); |
ihexx | 17:a29ce6fc667c | 84 | DigitalIn rSwitch(PORT_TURN_SIGNAL_SWITCH_RIGHT); |
ihexx | 17:a29ce6fc667c | 85 | |
ihexx | 17:a29ce6fc667c | 86 | PwmOut lLed(PORT_TURN_SIGNAL_LED_LEFT); |
ihexx | 17:a29ce6fc667c | 87 | PwmOut rLed(PORT_TURN_SIGNAL_LED_RIGHT); |
ihexx | 17:a29ce6fc667c | 88 | |
ihexx | 17:a29ce6fc667c | 89 | static inline void hotLoop(){ |
ihexx | 17:a29ce6fc667c | 90 | int a = lSwitch.read(); |
ihexx | 17:a29ce6fc667c | 91 | int b = rSwitch.read(); |
ihexx | 19:2044bb5d7f29 | 92 | |
ihexx | 19:2044bb5d7f29 | 93 | //If both switches are switched on |
ihexx | 19:2044bb5d7f29 | 94 | //then flash both indicator LEDs at a rate of 2Hz (hazard mode) |
ihexx | 17:a29ce6fc667c | 95 | if(a&&b){ |
ihexx | 20:202e0046527e | 96 | lLed.period(0.5f); |
ihexx | 20:202e0046527e | 97 | rLed.period(0.5f); |
ihexx | 17:a29ce6fc667c | 98 | } |
ihexx | 17:a29ce6fc667c | 99 | else{ |
ihexx | 17:a29ce6fc667c | 100 | lLed.period(1.0f); |
ihexx | 17:a29ce6fc667c | 101 | rLed.period(1.0f); |
ihexx | 17:a29ce6fc667c | 102 | } |
ihexx | 17:a29ce6fc667c | 103 | a ? lLed.write(0.5f) : lLed.write(0.0f); |
ihexx | 17:a29ce6fc667c | 104 | b ? rLed.write(0.5f) : rLed.write(0.0f); |
ihexx | 17:a29ce6fc667c | 105 | } |
ihexx | 17:a29ce6fc667c | 106 | |
ihexx | 17:a29ce6fc667c | 107 | } |
ihexx | 17:a29ce6fc667c | 108 | |
ihexx | 17:a29ce6fc667c | 109 | |
ihexx | 17:a29ce6fc667c | 110 | |
ihexx | 13:ab52f46c98ab | 111 | namespace task_group_1{ |
ihexx | 13:ab52f46c98ab | 112 | Thread thread; |
ihexx | 13:ab52f46c98ab | 113 | const float freq = 2.0f; //hz |
ihexx | 13:ab52f46c98ab | 114 | |
ihexx | 13:ab52f46c98ab | 115 | |
ihexx | 13:ab52f46c98ab | 116 | void runTask(){ |
ihexx | 13:ab52f46c98ab | 117 | Timer executionTimer,sleepTimer; |
ihexx | 13:ab52f46c98ab | 118 | executionTimer.reset(); |
ihexx | 13:ab52f46c98ab | 119 | sleepTimer.reset(); |
ihexx | 17:a29ce6fc667c | 120 | |
ihexx | 13:ab52f46c98ab | 121 | display::init(); |
ihexx | 17:a29ce6fc667c | 122 | brakeIndicator::init(); |
ihexx | 17:a29ce6fc667c | 123 | speedIndicator::init(); |
ihexx | 13:ab52f46c98ab | 124 | |
ihexx | 19:2044bb5d7f29 | 125 | const int const_delay = int((1000.0f/freq)+0.5f); //ideal sched delay |
ihexx | 19:2044bb5d7f29 | 126 | int dynamic_delay = const_delay; //real sched delay (updated in loop) |
ihexx | 19:2044bb5d7f29 | 127 | |
ihexx | 19:2044bb5d7f29 | 128 | int tick = 0; //freq subsampler |
ihexx | 19:2044bb5d7f29 | 129 | #if DEBUG_MODE |
ihexx | 19:2044bb5d7f29 | 130 | int max_exec_time = 0; //for logging |
ihexx | 19:2044bb5d7f29 | 131 | #endif |
ihexx | 17:a29ce6fc667c | 132 | |
ihexx | 13:ab52f46c98ab | 133 | while(true){ |
ihexx | 19:2044bb5d7f29 | 134 | //Determine scheduling compensators: |
ihexx | 19:2044bb5d7f29 | 135 | //1: release time drift |
ihexx | 19:2044bb5d7f29 | 136 | //2: execution time |
ihexx | 13:ab52f46c98ab | 137 | sleepTimer.stop(); |
ihexx | 13:ab52f46c98ab | 138 | executionTimer.start(); |
ihexx | 13:ab52f46c98ab | 139 | int sleepTime = sleepTimer.read_ms(); |
ihexx | 13:ab52f46c98ab | 140 | const int drift = ((sleepTime - dynamic_delay) > 0)? |
ihexx | 13:ab52f46c98ab | 141 | (sleepTime - dynamic_delay) : 0; |
ihexx | 13:ab52f46c98ab | 142 | |
ihexx | 13:ab52f46c98ab | 143 | |
ihexx | 19:2044bb5d7f29 | 144 | // Run all tasks-------------- |
ihexx | 17:a29ce6fc667c | 145 | brakeIndicator::hotLoop(); |
ihexx | 16:0ada6cbd41e2 | 146 | |
ihexx | 17:a29ce6fc667c | 147 | static const int tick_interval_sIndicator = int((freq/speedIndicator::freq)+0.5f); |
ihexx | 17:a29ce6fc667c | 148 | if (!(tick%tick_interval_sIndicator)){ |
ihexx | 17:a29ce6fc667c | 149 | speedIndicator::hotLoop(); |
ihexx | 17:a29ce6fc667c | 150 | sideLights::hotLoop(); |
ihexx | 17:a29ce6fc667c | 151 | } |
ihexx | 17:a29ce6fc667c | 152 | static const int tick_interval_tSignal = int((freq/turnSignal::freq)+0.5f); |
ihexx | 17:a29ce6fc667c | 153 | if (!(tick%tick_interval_tSignal)){ |
ihexx | 17:a29ce6fc667c | 154 | turnSignal::hotLoop(); |
ihexx | 17:a29ce6fc667c | 155 | } |
ihexx | 17:a29ce6fc667c | 156 | display::hotLoop(); |
ihexx | 19:2044bb5d7f29 | 157 | //--------------Completed tasks |
ihexx | 13:ab52f46c98ab | 158 | |
ihexx | 14:8a6c20435523 | 159 | tick++; |
ihexx | 13:ab52f46c98ab | 160 | executionTimer.stop(); |
ihexx | 13:ab52f46c98ab | 161 | int exec_time = executionTimer.read_ms(); |
ihexx | 19:2044bb5d7f29 | 162 | |
ihexx | 13:ab52f46c98ab | 163 | |
ihexx | 13:ab52f46c98ab | 164 | #if DEBUG_MODE |
ihexx | 19:2044bb5d7f29 | 165 | //Debug Logs (once per dequeue call to avoid memory issues) |
ihexx | 19:2044bb5d7f29 | 166 | if (exec_time > max_exec_time) max_exec_time=exec_time; |
ihexx | 15:524de2b2ef8e | 167 | static const int tick_interval_debug_log = int((freq/dequeueMail::freq)+0.5f); |
ihexx | 15:524de2b2ef8e | 168 | if (!(tick%tick_interval_debug_log)){ |
ihexx | 17:a29ce6fc667c | 169 | runTimeParams::debugAccess.lock(); |
ihexx | 17:a29ce6fc667c | 170 | *runTimeParams::debugLog += "task_group_1," + to_string(max_exec_time) + "," |
ihexx | 14:8a6c20435523 | 171 | + to_string(sleepTime) + "," |
ihexx | 14:8a6c20435523 | 172 | + to_string(drift) + "\n\r"; |
ihexx | 17:a29ce6fc667c | 173 | runTimeParams::debugAccess.unlock(); |
ihexx | 14:8a6c20435523 | 174 | } |
ihexx | 17:a29ce6fc667c | 175 | #else |
ihexx | 17:a29ce6fc667c | 176 | static const int tick_interval_debug_log = 1; |
ihexx | 13:ab52f46c98ab | 177 | #endif |
ihexx | 19:2044bb5d7f29 | 178 | //Reset tick count |
ihexx | 17:a29ce6fc667c | 179 | static const int tick_LCM = |
ihexx | 17:a29ce6fc667c | 180 | tick_interval_debug_log* |
ihexx | 17:a29ce6fc667c | 181 | tick_interval_sIndicator* |
ihexx | 17:a29ce6fc667c | 182 | tick_interval_tSignal; |
ihexx | 17:a29ce6fc667c | 183 | |
ihexx | 17:a29ce6fc667c | 184 | if (tick==tick_LCM) tick=0; |
ihexx | 17:a29ce6fc667c | 185 | |
ihexx | 19:2044bb5d7f29 | 186 | |
ihexx | 13:ab52f46c98ab | 187 | executionTimer.reset(); |
ihexx | 13:ab52f46c98ab | 188 | sleepTimer.reset(); |
ihexx | 13:ab52f46c98ab | 189 | sleepTimer.start(); |
ihexx | 19:2044bb5d7f29 | 190 | //compensate for delays |
ihexx | 13:ab52f46c98ab | 191 | dynamic_delay = const_delay - (exec_time + drift); |
ihexx | 13:ab52f46c98ab | 192 | Thread::wait(dynamic_delay); |
ihexx | 13:ab52f46c98ab | 193 | } |
ihexx | 13:ab52f46c98ab | 194 | } |
ihexx | 13:ab52f46c98ab | 195 | } |