Gershom Agim / Mbed 2 deprecated RTOS_EMBEDDED

GIU\ZF

Dependencies:   MCP23017 WattBob_TextLCD mbed-rtos mbed

Fork of rtos_basic by mbed official

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?

UserRevisionLine numberNew 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 }