main.cpp

00001 /*****************************************************************************************************
00002 Assigment 3 Embedded Software
00003                                                                                               
00004 This Software is a Car Control Software as no Real Car can be connected it Simulates a car as well.
00005 For the Display it uses an LCD Screen
00006 For the Speedometer it uses a Servo
00007 The Emulation and Synchronisation is implemented using TimerThreads and Semaphores
00008 for the reason of existing thread limits, conventional timers are helping to slicing timeslots
00009 
00010 @author Xavier Gouesnard
00011 H00258183
00012 ******************************************************************************************************/
00013 #include "mbed.h"
00014 #include "MCP23017.h"                                               // include 16-bit parallel I/O header file
00015 #include "WattBob_TextLCD.h"                                        // include 2*16 character display header file
00016 #include "rtos.h"
00017 #include "Servo.h"
00018 #include <deque>
00019 
00020 //#define OS_TIMERCBQS 20 // define a new number of timers we want +1 timer!
00021 //#define OS_TASKCNT 20 // maximum threads
00022 
00023 MCP23017            *par_port;                                      // pointer to 16-bit parallel I/O object
00024 WattBob_TextLCD     *lcd;                                           // pointer to 2*16 chacater LCD object
00025 Serial               serpc(USBTX, USBRX);                            // serial usb connection tx, rx
00026 AnalogIn AinBreak(p18);                                             // Port for the Break Value
00027 AnalogIn AinAccel(p19);                                             // Port for the Accelerator Value
00028 DigitalIn DinSwitchEngine(p11);                                     // Port for the Engine Switch
00029 DigitalIn DinSwitchLight(p12);                                      // Port for the Light Switch
00030 DigitalIn DinSwitchRindic(p13);                                     // Port for the Right Indicator
00031 DigitalIn DinSwitchLindic(p15);                                     // Port for the Left Indicator
00032 Servo Odometer(p21); 
00033 DigitalOut LEDSpeedWarning(p8);
00034 DigitalOut DoutLEDLight(LED1);                                      // Output Port for LED1
00035 DigitalOut DoutLEDLeft(LED2);                                       // Output Port for LED2
00036 DigitalOut DoutLEDRight(LED3);                                      // Output Port for LED3
00037 DigitalOut DoutLEDEngine(LED4);                                     // Output Port for LED4
00038 void Timer1_void(void const *args);                                 // Timer 1
00039 void Timer2_void(void const *args);                                 // Timer 2
00040 void Timer3_void(void const *args);                                 // Timer 3
00041 
00042 
00043 
00044 // Task Functions
00045 void Task_1_break_accelerate();
00046 void Task_2_read_show_engine_state();
00047 void Task_3_show_odometer();
00048 void Task_4_speed_warning();
00049 void Task_5_update_odometer();
00050 void Task_6_fill_mail_queue();
00051 void Task_7_dump_mail_to_serial();
00052 void Task_8_read_single_side_light();
00053 void Task_9_read_indicators();
00054 void Task_10_calc_avg_speed();
00055 void Task_11_emulate_car();
00056 
00057 // Init Variables
00058 int Convert_Hz_to_Ms(double Hz);
00059 float accerlator(0);
00060 float speed(0);
00061 float avgSpeed(0);
00062 float brake(0);
00063 float dist(0);
00064 bool engine(0);
00065 bool indicator_L(1);
00066 bool indicator_R(1);
00067 bool sw_timer1(0);
00068 bool sw_timer11(0);
00069 bool sw_timer2(0);
00070 bool sw_timer21(0);
00071 int sw_timer3(4);                                                   // sw_timer3 initalize with a first run
00072 
00073 std::deque<float> AvgSpeedDB;                                       // used for storing the average speed
00074 Semaphore SemAvgSpeedDB(1);                                         // declare used Semaphores
00075 Semaphore SemAvgSpeed(1);
00076 Semaphore SemSpeed(1);
00077 Semaphore SemBreak_Accelerate(1);
00078 Semaphore SemDistance(1);
00079 Semaphore SemEngine(1);
00080 Semaphore SemMailCnT(1);
00081  
00082  
00083 typedef struct {
00084 float    speed;
00085 float    accel;
00086 float brake;
00087 } mail_t;
00088 int mailcounter(0);                                                 // counts the mails in the queue
00089 Mail<mail_t, 100> mail_box;                                         // the mail queue has a maximum size of 100 mails
00090 
00091 int main()
00092 {
00093 // 10.0 Hz = 00100 ms
00094 // 2.0  Hz = 00500 ms
00095 // 5.0  Hz = 00200 ms
00096 // 1.0  Hz = 01000 ms
00097 // 0.5  Hz = 02000 ms
00098 // 0.2  Hz = 05000 ms
00099 // 0.05 Hz = 20000 ms
00100 
00101 
00102 
00103 
00104     serpc.baud(19200);                                              // setup the bautrate
00105     serpc.printf("Init Software\r\n");
00106     par_port = new MCP23017(p9, p10, 0x40);                         // initialise 16-bit I/O chip (0x40 = 64)
00107     lcd = new WattBob_TextLCD(par_port);                            // initialise 2*26 char display
00108     par_port->write_bit(1,BL_BIT);                                  // turn LCD backlight ON
00109     lcd->cls();                                                     // clear display
00110     lcd->locate(0,0);                                               // set cursor to location (0,0) - top left corner
00111     RtosTimer Timer1(Timer1_void,osTimerPeriodic,(void *)NULL);     // create the necesarry timers to overcome a thread issue (max threads)
00112     Timer1.start(Convert_Hz_to_Ms(20.0));
00113     RtosTimer Timer2(Timer2_void,osTimerPeriodic,(void *)NULL);
00114     Timer2.start(Convert_Hz_to_Ms(2.0));
00115     RtosTimer Timer3(Timer3_void,osTimerPeriodic,(void *)NULL);
00116     Timer3.start(Convert_Hz_to_Ms(0.2));
00117 Thread::wait(osWaitForever);
00118 }
00119 
00120 /*
00121 ##############################################################
00122 Timer 1 runs at 10 Hz
00123     Task_11_emulate_car();
00124     Task_1_break_accelerate();
00125     Task_10_calc_avg_speed();
00126 #############################################################
00127 */
00128 
00129 void Timer1_void(void const *args)
00130 {
00131 Task_11_emulate_car();                                           // runs every time, so at 10 Hz
00132     sw_timer1 = !sw_timer1;
00133 if(sw_timer1) {                                                  // runs just every second time, so at 10 hz
00134 Task_1_break_accelerate();
00135         sw_timer11 = !sw_timer11;
00136 if(sw_timer11) {                                                 // runs just every fourth time, so at 0.5 hz
00137 Task_10_calc_avg_speed();
00138         }
00139     }
00140 }
00141 
00142 /*
00143 ##############################################################
00144 Timer 2 runs at 2 Hz, but starts tasks at 2 Hz, 1 Hz, 0.5 Hz
00145     Task_2_read_show_engine_state();
00146     Task_5_update_odometer();
00147     Updates Indicators
00148     Task_3_show_odometer();
00149     Task_8_read_single_side_light();
00150     Task_4_speed_warning();
00151     Task_9_read_indicators();
00152 #############################################################
00153 */
00154 
00155 
00156 void Timer2_void(void const *args) // timer runs at 2 hz
00157 {
00158     
00159 Task_2_read_show_engine_state();
00160 Task_5_update_odometer();
00161     sw_timer2 = !sw_timer2;
00162     
00163 if(indicator_L && indicator_R ) {                               // Sets the Left and Right Inidcators
00164         DoutLEDLeft=!DoutLEDRight;                              // needs to get the inverted status of led1 before led1 is changed
00165         DoutLEDRight=!DoutLEDRight;
00166         
00167     } else if (!indicator_R && !indicator_L) {
00168         DoutLEDLeft=0;
00169         DoutLEDRight=0;
00170     }
00171     
00172 if(sw_timer2) 
00173 {                                                               // runs just every second time, so at 1 hz
00174 Task_3_show_odometer();
00175 Task_8_read_single_side_light();
00176         sw_timer21 = !sw_timer21;
00177         
00178 if (!indicator_R && indicator_L) 
00179 {                                                               // switch the left / right indicator
00180             DoutLEDRight=0;
00181             DoutLEDLeft=!DoutLEDLeft;
00182         } else if(indicator_R && !indicator_L) {
00183             DoutLEDRight=!DoutLEDRight;
00184             DoutLEDLeft=0;
00185         }
00186 if(sw_timer21) {                                            // runs just every second time, so at 0.5 hz
00187 Task_4_speed_warning();
00188 Task_9_read_indicators();
00189         }
00190     }
00191 }
00192 /*
00193 ##############################################################
00194 Timer 3 runs at 0.2 Hz, but starts tasks at 0.2 Hz and 0.05 Hz
00195     Task_6_fill_mail_queue();
00196     Task_7_dump_mail_to_serial();
00197 ##############################################################
00198 */
00199 void Timer3_void(void const *args)                                  // timer runs at 0.2 hz
00200 {
00201 Task_6_fill_mail_queue();
00202 if((sw_timer3%4)==0) {                                          // task runs at 0.05 Hz
00203 Task_7_dump_mail_to_serial();                                // dump the queue to serial
00204         sw_timer3=0;                                                // reset the timer
00205     }
00206     sw_timer3++;
00207 }
00208 /*
00209 Reads the brake / acceleration of the car
00210 */
00211 void Task_1_break_accelerate()
00212 {
00213 // Let the Semaphores wait
00214     SemBreak_Accelerate.wait();
00215     accerlator = AinAccel;                                              // save the accerlator value
00216     brake = AinBreak;                                                   // save the brake value
00217     
00218 // Let the Semaphores release
00219     SemBreak_Accelerate.release();
00220 }
00221 /*
00222 Reads the Engine On/Off Switch and displays its state
00223 */
00224 void Task_2_read_show_engine_state()
00225 {
00226 // Let the Semaphores wait
00227     SemEngine.wait();
00228     engine = DinSwitchEngine;                                           // read the engine state
00229     DoutLEDEngine = engine;                                             // write the engine state
00230 // Let the Semaphores release
00231     SemEngine.release();
00232 }
00233 /*
00234 Updates the Odometer (Servo Motor)
00235 */
00236 void Task_3_show_odometer()
00237 {
00238 // Let the Semaphores wait
00239     SemAvgSpeed.wait();                                                 
00240     Odometer = avgSpeed/250.0;                                          // Calculate the odometer
00241 // Let the Semaphores release
00242     SemAvgSpeed.release();
00243 }
00244 
00245 /*
00246 Indicates a Speed warning at 75 Mph
00247 */
00248 void Task_4_speed_warning()
00249 {
00250 // Let the Semaphores wait
00251     SemAvgSpeed.wait();
00252 if(avgSpeed>70.0)                                                   // check our speed
00253         LEDSpeedWarning = !LEDSpeedWarning;                             // and switch the Warning on/off
00254 else
00255         LEDSpeedWarning = 0;
00256 // Let the Semaphores release
00257     SemAvgSpeed.release();
00258 }
00259 /*
00260 Updates the LCD Display
00261 */
00262 void Task_5_update_odometer()
00263 {
00264 // Let the Semaphores wait
00265     SemDistance.wait();
00266     SemAvgSpeed.wait();
00267     lcd->locate(0,0);                                               // set cursor to location (0,0) - top left corner
00268     lcd->printf("s: %5.0f",avgSpeed);
00269     lcd->locate(1,0);
00270     lcd->printf("d: %5.0f",dist);
00271 // Let the Semaphores release
00272     SemDistance.release();
00273     SemAvgSpeed.release();
00274 }
00275 /*
00276 Reads the Left and Right Inidcator
00277 */
00278 void Task_6_fill_mail_queue()
00279 {
00280 // Let the Semaphores wait
00281     SemMailCnT.wait();
00282     SemBreak_Accelerate.wait();
00283     SemSpeed.wait();
00284 mail_t *mail = mail_box.alloc();                                            // reserve the space for our new message
00285     mail->speed = speed;                                                        // fill with values
00286     mail->accel = accerlator;
00287     mail->brake = brake;
00288     mail_box.put(mail);                                                         // put the new message into the mail queue
00289     mailcounter++;
00290 // Let the Semaphores release
00291     SemBreak_Accelerate.release();
00292     SemSpeed.release();
00293     SemMailCnT.release();
00294 }
00295 /*
00296 Reads the Mail Queue and Sends the Content to the Serial Port
00297 */
00298 void Task_7_dump_mail_to_serial()
00299 {
00300 // Let the Semaphores wait
00301     SemMailCnT.wait();
00302 while(mailcounter) {                                                        // as long as we got mail
00303         osEvent evt = mail_box.get();                                           // we are getting them
00304 if (evt.status == osEventMail) {
00305 mail_t *mail = (mail_t*)evt.value.p;                                // print the mail to serial
00306             serpc.printf("\nspeed: %.0f \n\r"   , mail->speed);
00307             serpc.printf("accerlator: %.2f\n\r"     , mail->accel);
00308             serpc.printf("brake: %.2f\n\r", mail->brake);
00309             mail_box.free(mail);                                                // clear up the mailbox
00310         }
00311         mailcounter--;
00312     }
00313 // Release the Semaphores
00314     SemMailCnT.release();
00315 }
00316 
00317 /*
00318 Single Side Light
00319 */
00320 void Task_8_read_single_side_light()
00321 {
00322     DoutLEDLight = DinSwitchLight;                                             // Reading the value
00323 }
00324 
00325 /*
00326 Reads the Left and Right Inidcator
00327 */
00328 void Task_9_read_indicators()
00329 {
00330     indicator_R = DinSwitchRindic;                                            // Reading the value
00331     indicator_L = DinSwitchLindic;                                            // Reading the value
00332 }
00333 
00334 /*
00335 Calculates the Average Speed
00336 */
00337 void Task_10_calc_avg_speed()
00338 {
00339 // Let the Semaphores wait
00340     SemAvgSpeed.wait();
00341     SemAvgSpeedDB.wait();
00342 float sum(0);
00343 for(deque<float>::const_iterator i = AvgSpeedDB.begin(); i != AvgSpeedDB.end(); ++i)
00344         sum+= *i;                                                             // calculate the average by iterating over the queue
00345     avgSpeed = sum/AvgSpeedDB.size();
00346 // Release the Semaphores
00347     SemAvgSpeedDB.release();
00348     SemAvgSpeed.release();
00349 }
00350 
00351 /*
00352 Emulates the car
00353 */
00354 void Task_11_emulate_car()
00355 {
00356 // Let the Semaphores wait
00357     SemAvgSpeed.wait();
00358     SemAvgSpeedDB.wait();
00359     SemDistance.wait();
00360     SemBreak_Accelerate.wait();
00361     SemSpeed.wait();
00362     SemEngine.wait();
00363 if(accerlator<=brake || !engine)                                             // are we braking more than accelerating? is the engine on?
00364         speed = 0;
00365 else
00366         speed = (accerlator-brake) *0.5 +speed;
00367 if(speed>250)
00368         speed=250;                                                           // maximum speed
00369 if(AvgSpeedDB.size()>=4)                                                     // if we already got 4 values, we have to
00370         AvgSpeedDB.pop_front();                                              // make space by deleting the oldest value
00371     AvgSpeedDB.push_back(speed);                                             // safe a new reading
00372     dist += speed * 1.0/20.0;                                                // runs at 20 Hz so we have to take this into account
00373 // Release the Semaphores
00374     SemDistance.release();
00375     SemAvgSpeed.release();
00376     SemAvgSpeedDB.release();
00377     SemBreak_Accelerate.release();
00378     SemSpeed.release();
00379     SemEngine.release();
00380 }
00381 
00382 /*
00383 Function used for converting Hz to Ms for a Steps
00384 */
00385 int Convert_Hz_to_Ms(double Hz)                                              // Convert Hz into ms
00386 {
00387 return 1000.0 / Hz;
00388 }