main.cpp

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