task_group2.cpp

00001 #include "core.h"
00002 namespace getControls{
00003     //Read Accelerator and Brake
00004     AnalogIn brake(PORT_BRAKE);
00005     AnalogIn accel(PORT_ACCEL);
00006     const float freq = 10.0f;
00007     
00008     static inline void hotLoop(){
00009         runTimeParams::liveAccess.lock();
00010         runTimeParams::brakeForce = brake.read();
00011         runTimeParams::accelForce = accel.read();
00012         runTimeParams::liveAccess.unlock();
00013     }
00014 }
00015 namespace getIgnition{
00016     //Read engine on/off switch and show current state on a LED
00017     const float freq = 2; //hz
00018     DigitalIn ignition(PORT_IGNITION);
00019     DigitalOut led1(IGNITION_LED);
00020     
00021     static inline void hotLoop(){
00022             led1 = ignition.read();
00023     }
00024 }
00025 namespace carSimulator{
00026     //Compute speed given car controls and timing data
00027     const float freq = 20.0f;
00028     static inline void hotLoop(){
00029         const float friction = 0.1f; //constant retardation
00030         runTimeParams::liveAccess.lock();
00031         
00032         //Check if ignition is on, and don't accelerate if it isn't
00033         float accel;
00034         if (getIgnition::ignition.read()){
00035             accel = runTimeParams::accelForce - 
00036             (runTimeParams::brakeForce+friction);
00037         }
00038         else{
00039             accel= -(runTimeParams::brakeForce+friction);
00040         }
00041         
00042         static int i = 0; //iterator for speed
00043         const int i_prev = i;
00044         i = (i>=3)? 0: i+1;
00045         //Store to speed array in round robin format
00046         float tmpSpeed = accel + runTimeParams::speed[i_prev];
00047         tmpSpeed = (tmpSpeed>0)?tmpSpeed:0;
00048         tmpSpeed = (tmpSpeed>200)?200:tmpSpeed;
00049         runTimeParams::speed[i] = tmpSpeed;
00050         runTimeParams::odometer += tmpSpeed*0.01f;
00051         runTimeParams::liveAccess.unlock();
00052         }
00053 }
00054 namespace filterSpeed{
00055     //Filter speed with averaging filter
00056     static const float freq = 5; //hz 
00057     static inline void hotLoop(){
00058             runTimeParams::liveAccess.lock();
00059             runTimeParams::avgSpeed = (runTimeParams::speed[0] +
00060                             runTimeParams::speed[1] +
00061                             runTimeParams::speed[2])/3;
00062             runTimeParams::liveAccess.unlock();
00063     } 
00064 }
00065 
00066 
00067 namespace task_group_2{
00068     Thread thread;
00069     const float freq = 20.0f; //hz
00070 
00071     void runTask(){
00072         //Init
00073         Timer executionTimer,sleepTimer;
00074         executionTimer.reset();
00075         sleepTimer.reset();
00076         
00077         const int const_delay = int((1000.0f/freq)+0.5f); //ideal scheduling rate
00078         int dynamic_delay = const_delay; //compensating for release/execution time
00079         int tick = 0; //downsample task frequencies
00080         
00081         #if DEBUG_MODE
00082         int max_exec_time = 0; //for logging
00083         #endif
00084         
00085         while(true){
00086             //Determine scheduling compensators:
00087                 //1: release time drift 
00088                 //2: execution time 
00089             sleepTimer.stop();
00090             executionTimer.start();
00091             int sleepTime = sleepTimer.read_ms();
00092             const int drift = ((sleepTime - dynamic_delay) > 0)?
00093                                         (sleepTime - dynamic_delay) : 0;
00094             
00095             
00096             // Run all tasks--------------
00097             carSimulator::hotLoop();
00098             
00099             static const int tick_interval_filter = int((freq/filterSpeed::freq)+0.5f);
00100             if (!(tick%tick_interval_filter)) filterSpeed::hotLoop();
00101             
00102             static const int tick_interval_controls = int((freq/getControls::freq)+0.5f);
00103             if (!(tick%tick_interval_controls)) getControls::hotLoop();
00104             
00105             static const int tick_interval_ignitionLED = int((freq/getIgnition::freq)+0.5f);
00106             if (!(tick%tick_interval_ignitionLED )) getIgnition::hotLoop();
00107             //-------------Completed tasks
00108             
00109             tick++;
00110             executionTimer.stop();
00111             int exec_time = executionTimer.read_ms();
00112             
00113             
00114             #if DEBUG_MODE
00115             //Debug Logs (once per dequeue call to avoid memory issues)
00116             if (exec_time > max_exec_time) max_exec_time=exec_time;
00117             static const int debug_log_interval = int(freq/dequeueMail::freq);
00118             if (!(tick%debug_log_interval)){
00119                 runTimeParams::debugAccess.lock();
00120                 *runTimeParams::debugLog += "task_group_2," + to_string(max_exec_time) + ","
00121                             + to_string(sleepTime) + ","
00122                             + to_string(drift) + "\n\r";
00123                 exec_time = 0;
00124                 runTimeParams::debugAccess.unlock();
00125             }
00126             #else
00127             static const int debug_log_interval = 1;
00128             #endif
00129             
00130             //Reset tick count
00131             static const int tick_LCM = 
00132                 debug_log_interval*
00133                 tick_interval_ignitionLED*
00134                 tick_interval_controls*
00135                 tick_interval_filter;
00136                 
00137             if (tick==tick_LCM) tick=0;
00138             
00139             executionTimer.reset();
00140             sleepTimer.reset();
00141             sleepTimer.start();
00142             //compensate for delays
00143             dynamic_delay = const_delay -(exec_time + drift);
00144             Thread::wait(dynamic_delay);
00145         }   
00146     }
00147 }