main.cpp

00001 /* #####################################################################
00002                                main.cpp
00003                                ---------
00004                 
00005                      Embedded Software - Assignment 3
00006                      --------------------------------
00007  
00008  Written by:        Steven Kay
00009  
00010  Date:              March 2016
00011  
00012  Function:          This code operates to simulate the operation of a Car's
00013                     control system. It does so by taking external input for 
00014                     elements of a Car; Light Indicators, Engine State and Pedal values
00015                     and using a simple Car Simulation model, derives a speed
00016                     from which several tasks are included to act upon.
00017                     An average speed is generated at a given frequency 
00018                     and a total distance is estimated using this average speed.
00019                     Output comes in the form of LED's when specific indicators 
00020                     are true, and on a LCD screen and Servo Wiper to indicate speed.
00021                     Furthermore, a PC Connection is established and a data dump of
00022                     current Accelerometer, Brake and Average speed is presented
00023                     to the user every 20 seconds.
00024                     
00025                     This system is constructed using the MBED-RTOS and 
00026                     as such, each Task has an associated frequency, found in main, below.
00027                     Note that no priority is given to the tasks and no 
00028                     extra scheduler controls the synchronisation of tasks,
00029  
00030  ##################################################################### */
00031  
00032 #include "mbed.h"
00033 #include "rtos.h"
00034 #include "MCP23017.h"
00035 #include "WattBob_TextLCD.h"
00036 
00037 // ============================================================================
00038 // Define Statements
00039 // ============================================================================
00040 
00041 // LCD Definitions
00042 #define BACK_LIGHT_ON(INTERFACE) INTERFACE->write_bit(1,BL_BIT)
00043 #define BACK_LIGHT_OFF(INTERFACE) INTERFACE->write_bit(0,BL_BIT)
00044 
00045 // General Logical Assignments
00046 #define HIGH 1
00047 #define LOW 0
00048 
00049 // Car Sim, Maximum Car Speed (mph)
00050 #define MAX_SPEED 100
00051 
00052 // ============================================================================
00053 // MBED Pin Assignments
00054 // ============================================================================
00055 
00056 // System Inputs
00057 // ----------------------------------------------------------------------------
00058 
00059 // Analog Channels
00060 AnalogIn Brake(p19);                        // Brake Pedal
00061 AnalogIn Accelerometer(p20);                // Accelerator Pedal
00062 
00063 // Digitial Channels
00064 DigitalIn EngineState(p18);                 // Engine State Switch
00065 DigitalIn LeftIndicator(p17);               // Left Indicator Switch
00066 DigitalIn RightIndicator(p16);              // Right Indicator Switch
00067 DigitalIn SideLightIndicator(p15);          // Side Light Indicator
00068 
00069 
00070 // System Outputs
00071 // ----------------------------------------------------------------------------
00072 
00073 // LED Indicators (Steady State)
00074 DigitalOut EngineStateInd(LED1);            // Engine State LED
00075 DigitalOut SideLightInd(LED2);              // Side Light LED
00076 
00077 // LED Indicators (Flashing)
00078 PwmOut LeftLightInd(LED3);                  // Left Indicator LED
00079 PwmOut RightLightInd(LED4);                 // Right Indicator LED
00080 PwmOut OverSpeedInd(p22);                   // OverSpeed LED
00081 
00082 // Servo Output
00083 PwmOut AvSpeedWiper(p21);                   // Average Speed Wiper
00084 
00085 // USB Connection to PC
00086 Serial PCConn(USBTX,USBRX);                 // Connection to PC
00087 
00088 // LCD Objects
00089 MCP23017 *par_port;                         // Object pointing to Expander
00090 WattBob_TextLCD *lcd;                       // LCD Connection
00091 
00092 // ============================================================================
00093 // Global Data Structure Declerations
00094 // ============================================================================
00095 
00096 // Raw Data Structure
00097 // ----------------------------------------------------------------------------
00098 
00099 // CarRawData  is a global memory area and contaims an instance of the Raw data 
00100 // populated by Task1 and can be accessed through locking Mutex - rawDataMutex
00101 typedef struct
00102 {
00103     bool EngineState;
00104     float RawAccelerometer;
00105     float RawBraking;
00106 } CarRawData;
00107 
00108 // Create Mutex to control access to CarRawData instance
00109 Mutex rawDataMutex;
00110 
00111 // Create Instance of CarRawData
00112 CarRawData rawData;
00113 
00114 // Speed Data Structure
00115 // ----------------------------------------------------------------------------
00116 
00117 // CarSpeedData is a global memory area and contains an instance of the calculated
00118 // raw speed values and the index to the next available element in the array to be 
00119 // written to. rawSpeed is calculated by the CarSim and can be accessed through
00120 // locking Mutex - SpeedMutex
00121 typedef struct
00122 {
00123     float rawSpeed[3];
00124     int counter;
00125 } CarSpeedData;
00126 
00127 // Create Mutex to control access to CarSpeedData instance
00128 Mutex SpeedMutex;
00129 
00130 // Create instance of CarSpeedData
00131 CarSpeedData speedData;
00132 
00133 // Filtered Data Structure
00134 // ----------------------------------------------------------------------------
00135 
00136 float AverageSpeed;
00137 float totalDistance;
00138 
00139 // Create Mutex to control access to FilteredData
00140 Mutex filteredDataMutex;
00141 
00142 // Mail Queue Structure
00143 // ----------------------------------------------------------------------------
00144 
00145 // PCDumpData is a global memory area which is populated by Taak7 and used as
00146 // the structure within a MailQueue. Data contained is a copy from the current
00147 // state of the Control System at a given instance of time.
00148 typedef struct
00149 {
00150     float currentAverageSpeed;
00151     float currentAccelerometer;
00152     float currentBraking;
00153 } PCDumpData;
00154 
00155 // Construct a 100 Element Mail Queue structure
00156 Mail<PCDumpData,100> Memory_Dump;
00157 
00158 // Define a Counter to trakc number of entries to Mail Queue
00159 int MailQueueCounter;
00160 
00161 // Create Mutex to control access to the MailQueueCounter variable
00162 Mutex MailMutex;
00163 
00164 
00165 // ============================================================================
00166 // Car Simulation
00167 // ============================================================================
00168 
00169 // The CarSimulator Task updates the rawSpeed parameter at a frequenct of 20Hz
00170 void CarSimulator(void const *arg)
00171 {
00172     float newSpeed;
00173     
00174     // Load Shared resources into local variables within the Task
00175     // Shared Resources are; Accelerometer value, Braking value and Engine State
00176     rawDataMutex.lock();
00177     float currentAccelerometer = rawData.RawAccelerometer;
00178     float currentBrake = rawData.RawBraking;
00179     bool currentEngineState = rawData.EngineState;
00180     rawDataMutex.unlock();   
00181 
00182     // Run simple model which estimates the speed, as a fraction of the MAX SPEED
00183     // based on the percentage of either accelerator or brake pressed by the user
00184     // Further, newSpeed is set to 0 if the currentEngineState is equal to 0 (Engine off)
00185     newSpeed = currentAccelerometer*MAX_SPEED*(1-currentBrake)*currentEngineState;
00186     
00187     // Check Speed Counter's range, if out of bounds of array length, reset Counter
00188     // to 0
00189     // Data contained within Shared Resource therefore the SpeedMutex is used to control access
00190     SpeedMutex.lock();
00191     if(speedData.counter > 2)
00192     {
00193         speedData.counter = 0;
00194     }
00195     
00196     // Output a rawSpeed value to the next available index of rawSpeed[] and increment Counter
00197     speedData.rawSpeed[speedData.counter] = newSpeed;
00198     speedData.counter = speedData.counter++;
00199     SpeedMutex.unlock();
00200 }
00201 
00202 // ============================================================================
00203 // Control System Tasks
00204 // ============================================================================
00205 
00206 // Task1_ReadRawData gathers external inputs and updates the rawData structure
00207 // It operates at a frequency of 10Hz
00208 void Task1_ReadRawData(void const *arg)
00209 {
00210       // Lock Shared Resource - rawData
00211       // Update rawData elements directly from AnalogIn channel values
00212       // Unlock Shares Resource
00213       rawDataMutex.lock();
00214       rawData.RawBraking = Brake.read();
00215       rawData.RawAccelerometer = Accelerometer.read();
00216       rawDataMutex.unlock();
00217 }
00218 
00219 
00220 // Task2_ReadEngineState updates the rawData structure and operates ar a frequency of 2Hz
00221 void Task2_ReadEngineState(void const *arg)
00222 {
00223     // Get external input from DigitalIn Channel and store in local variable
00224     bool currentEngineState = EngineState.read();
00225     
00226     // Lock Shared Resource - rawData
00227     // Take a copy of the local variable currentEngineState and store into Global memory
00228     // Unlock Shared Resource
00229     rawDataMutex.lock();
00230     rawData.EngineState = currentEngineState;
00231     rawDataMutex.unlock();
00232     
00233     // Conduct logical check on local varialbe currentEngineState
00234     // if currentEngineState is HIGH, set EngineStateInd to HIGH
00235     // else set EngineStateInd LOW
00236     if(currentEngineState)
00237     {
00238         EngineStateInd = HIGH;
00239     }
00240     else
00241     {
00242         EngineStateInd = LOW;
00243     }
00244 }
00245        
00246        
00247 // Task3_CalcAvSpeed updates the AverageSpeed global varialbe and operates at a frequency of 5Hz    
00248 void Task3_CalcAvSpeed(void const *arg)
00249 {
00250     // Initialise local variable as 0.0
00251     float speedTotal = 0.0;
00252     
00253     // Lock Shared Resource - speedData
00254     // Calculate total from array of rawSpeed values and store locally
00255     // Unlock Shared Resource
00256     SpeedMutex.lock();
00257     for(int num = 0; num < 3; num++)
00258     {
00259         speedTotal = speedTotal + speedData.rawSpeed[num];
00260     }
00261     SpeedMutex.unlock();
00262 
00263     // Lock Shared Resource - AverageSpeed
00264     // Calculate average from local variable speedTotal and store result Globally into AverageSpeed
00265     // Unlock Shared Resource
00266     filteredDataMutex.lock();
00267     AverageSpeed = (speedTotal/3);
00268     filteredDataMutex.unlock();
00269 }
00270 
00271 
00272 // Task4_UpdateRCWiper takes the AverageSpeed global variable at a given time and outputs 
00273 // a representation of this through a Servo. It operates at a frequenct of 1Hz
00274 void Task4_UpdateRCWiper(void const *arg)
00275 {
00276     // Lock Shared Resource - AverageSpeed
00277     // Take local copy of AverageSpeed
00278     // Unlock Shared Resource
00279     filteredDataMutex.lock();
00280     float currentAverageSpeed = AverageSpeed;
00281     filteredDataMutex.unlock();
00282     
00283     // Update Servo Position based upon the local copy of AverageSpeed
00284     
00285     // Servo must be controlled in the range of 1000us to 2000us
00286     // Thus a base value of 1000 is included, and as currentAverageSpeed cannot exceed 100,
00287     // This is simply scaled by 10, to give a maximum of 2000, which allows Servo to operate
00288     // Over full operational range
00289     AvSpeedWiper.pulsewidth_us(1000+(10*currentAverageSpeed));
00290 }
00291 
00292 
00293 // Task5_OverspeedLED takes the 0th Element of the rawSpeed global variable and computes
00294 // a logical assessment to detect when the speed is greater than a preset of 70mph and 
00295 // indicate results through an LED. It operates at a frequency of 0.5Hz
00296 void Task5_OverspeedLED(void const *arg)
00297 {
00298     // Lock Shared Resource - speedData
00299     // Take local copy of rawSpeed[0]
00300     // // Unlock Shares Resource
00301     SpeedMutex.lock();
00302     float currentInstSpeed = speedData.rawSpeed[speedData.counter];
00303     SpeedMutex.unlock();
00304     
00305     // Using local copy of rawSpeed[0], if this is above preset threshold of 70,
00306     // OverSpeedInd is set HIGH, else it is set LOW
00307     if(currentInstSpeed > 70.0)
00308     {
00309         OverSpeedInd = HIGH;
00310     }
00311     else
00312     {
00313         OverSpeedInd = LOW;
00314     }
00315 }
00316 
00317 
00318 // Task6_UpdateOdometer takes the AverageSpeed global variable and calculates the
00319 // distance travelled by the Car over a known time increment (the delta of time between this task being ran)
00320 // Once calculated, the distance is stored globally and also, in conjunction with AverageSpeed, displayed onto
00321 // a LCD screen. It operates at a frequency of 2Hz
00322 void Task6_UpdateOdometer(void const *arg)
00323 {   
00324     // Lock Shared Varialbe - AverageSpeed
00325     // Take local copy of AverageSpeed
00326     // Unlock Shared Variable
00327     filteredDataMutex.lock();
00328     float currentAverageSpeed = AverageSpeed;
00329     filteredDataMutex.unlock();
00330     
00331     // Compute newTotalDistance from current TotalDistance and average speed
00332     
00333     // distance = oldDistance +(currentAverageSpeed*timeIncrement)
00334     // Note that timeIncrement (0.5 second) is converted from seconds to hours, 
00335     // To remain consistant with mph units
00336     
00337     // NOTE
00338     // totalDistance does not need to be protected by a Mutex as this is the only task which updates 
00339     // or uses the variable. It is global in order to keep track of a rolling total
00340     totalDistance = totalDistance + (currentAverageSpeed*(0.5/3600));
00341     
00342     // Output totalDistance and currentAverageSpeed to LCD
00343     lcd -> cls();
00344     lcd -> locate(0,0);
00345     lcd -> printf("Dist: %8.2f",totalDistance);
00346     lcd -> locate(1,0);
00347     lcd -> printf("Speed: %3.1f mph",currentAverageSpeed);
00348 }
00349 
00350 
00351 // Task7_SendToMailQueue takes global variables; AverageSpeed, RawAccelerometer and RawBraking and
00352 // creates a structure from them, then puts an instanc of this structure onto a Mail Queue, incrementing
00353 // a globally defined Counter as it does such. It operates with a frequency of 0.2Hz
00354 void Task7_SendToMailQueue(void const *arg)
00355 {
00356     // Lock Shared Resource - AverageSpeed
00357     // Take local copy of AverageSpeed
00358     // Unlock Shared Resource
00359     filteredDataMutex.lock();
00360     float currentAverageSpeed = AverageSpeed;
00361     filteredDataMutex.unlock();
00362     
00363     // Lock Shared Resource - rawData
00364     // Take local copy of RawAccelerometer and RawBraking
00365     // Unlock Shared Resource
00366     rawDataMutex.lock();
00367     float currentAccelerometer = rawData.RawAccelerometer;
00368     float currentBrake = rawData.RawBraking;
00369     rawDataMutex.unlock();  
00370     
00371     // Allocate Memory for instance of PCDumpData structure
00372     PCDumpData *currentPCDump = Memory_Dump.alloc();
00373     
00374     // Populate instance of PCDumpData with local copies of desired variables
00375     currentPCDump -> currentAverageSpeed = currentAverageSpeed;
00376     currentPCDump -> currentAccelerometer = currentAccelerometer;
00377     currentPCDump -> currentBraking = currentBrake;
00378     
00379     // Push instance of PCDumpData onto Mail Queue
00380     Memory_Dump.put(currentPCDump);  
00381     
00382     // Lock Shared Resource - MailQueueCounter
00383     // Increment MailQueueCounter
00384     // Unlock Shared Resource
00385     MailMutex.lock();
00386     MailQueueCounter++;
00387     MailMutex.unlock();  
00388 }
00389 
00390 
00391 // Task8_DumpSerial takes all of the instances of PCDumpData which have been pushed onto the Mail Queue
00392 // removes them from the Mail Queue and dumps their values over a PC serial connection. Once complete, the
00393 // counter which stores the number of elements in the Mail Queue is reset to 0. It operates at a frequency of 0.05Hz
00394 void Task8_DumpSerial(void const *arg)
00395 {
00396     // Lock Shared Resource - MailQueueCounter
00397     // Take local copy of MailQueueCounter
00398     // Unlock Shared Resource
00399     MailMutex.lock();
00400     int currentQueueCounter = MailQueueCounter;
00401     MailMutex.unlock();
00402     
00403     // Prompt State of Memory Dump
00404     PCConn.printf("Memory Dump\r\n");
00405     
00406     // For each instance of PCDumpData found in the Mail Queue, import the structure and store locally
00407     // Then print the contained values and free the element of the Mail Queue
00408     // Repeat for all indexes of the Mail Queue
00409     for(int num = 0; num < currentQueueCounter; num++)
00410     {
00411         osEvent evt = Memory_Dump.get();
00412         if(evt.status == osEventMail)
00413         {
00414             PCDumpData *currentPCDump = (PCDumpData*)evt.value.p;
00415             
00416             PCConn.printf("Av Speed: %f\r\nAcceler: %f\r\nBrake: %f\r\n\r\n",  currentPCDump -> currentAverageSpeed,
00417                                                                     currentPCDump -> currentAccelerometer,
00418                                                                     currentPCDump -> currentBraking);
00419             Memory_Dump.free(currentPCDump);
00420         }
00421     }
00422     
00423     // Lock Shared Resource - MailQueueCounter
00424     // Reset MailQueueCounter
00425     // Unlock Shared Resource
00426     MailMutex.lock();
00427     MailQueueCounter = 0;
00428     MailMutex.unlock();
00429 }
00430 
00431 
00432 // Task9_ReadSideLight takes external input from SideLightIndicator and conducts a logical test.
00433 // It operates at a frequency of 1Hz
00434 void Task9_ReadSideLight(void const *arg)
00435 {
00436     // If SideLightIndicator is HIGH, set SideLightInd to HIGH, else set to LOW
00437     if(SideLightIndicator)
00438     {
00439         SideLightInd = HIGH;
00440     }
00441     else
00442     {
00443         SideLightInd = LOW;  
00444     }  
00445 }
00446 
00447 
00448 // Task10_ReadIndicatorLights takes external input from LeftIndicator and RightIndicator and conducts a
00449 // logical test. It operates at a frequency of 0.5Hz
00450 void Task10_ReadIndicatorLights(void const *arg)
00451 {
00452 
00453     // If LeftIndicator Only is HIGH, flash LeftLightInd at a frequency of 1Hz, 50% Duty
00454     if(LeftIndicator && !RightIndicator)
00455     {
00456         LeftLightInd.period(1.0);
00457         LeftLightInd.pulsewidth(0.5);
00458         
00459         RightLightInd.period(1.0);
00460         RightLightInd.pulsewidth(0.0);
00461     }
00462     
00463     // If RightIndicator Only is HIGH, flash RightLightInd at a frequency of 1Hz, 50% Duty
00464     else if(!LeftIndicator && RightIndicator)
00465     {
00466         LeftLightInd.period(1.0);
00467         LeftLightInd.pulsewidth(0.0);
00468         
00469         RightLightInd.period(1.0);
00470         RightLightInd.pulsewidth(0.5);
00471     }
00472     
00473     // If LeftIndicator and RightIndicator  are HIGH, flash LeftLightInd and RightLightInd
00474     //  at a frequency of 2Hz, 50% Duty
00475     else if(LeftIndicator && RightIndicator)
00476     {
00477         LeftLightInd.period(0.5);
00478         RightLightInd.period(0.5);
00479         
00480         LeftLightInd.pulsewidth(0.25);
00481         RightLightInd.pulsewidth(0.25);
00482     }
00483     // Else, turn off both LeftLightInd and RightLightInd
00484     else
00485     {
00486         LeftLightInd.period(1.0);
00487         LeftLightInd.pulsewidth(0.0);
00488         
00489         RightLightInd.period(1.0);
00490         RightLightInd.pulsewidth(0.0);
00491     }
00492 }
00493       
00494       
00495 void InitSystem()
00496 {
00497     // Set AvSpeedWiper to 50Hz frequency, for Servo
00498     AvSpeedWiper.period_ms(20);
00499     
00500     // Initiate LCD by lighting Backlight
00501     par_port->write_bit(1,BL_BIT);
00502 
00503     // Initiate all Global variables to 0
00504     rawData.EngineState = 0;
00505     rawData.RawAccelerometer = 0.0;
00506     rawData.RawBraking = 0.0;
00507     speedData.counter = 0;
00508     speedData.rawSpeed[0] = 0.0;
00509     speedData.rawSpeed[1] = 0.0;
00510     speedData.rawSpeed[2] = 0.0;
00511     AverageSpeed = 0.0;
00512     totalDistance = 0.0;
00513     MailQueueCounter = 0;
00514 }
00515     
00516 
00517 // ============================================================================
00518 // Entry Point Thread
00519 // ============================================================================
00520 
00521 // Entry Point Thread, this shall be the initialiser for the Car System and 
00522 // Contains all of the Tasks and their associated properties, such as frequency.
00523 int main()
00524 {
00525  // Construct Objects for LCD
00526  par_port = new MCP23017(p9, p10, 0x40); // initialise 16-bit I/O chip
00527  lcd = new WattBob_TextLCD(par_port); // initialise 2*26 char display
00528 
00529  // Set PC Connection Baud Rate
00530  PCConn.baud(115200);
00531  
00532  // Initialise System, including Global Variables
00533  InitSystem();
00534   
00535 // Construct Tasks as RtosTimer objects
00536  RtosTimer CarSim(CarSimulator,osTimerPeriodic);
00537  RtosTimer Task1(Task1_ReadRawData,osTimerPeriodic);
00538  RtosTimer Task2(Task2_ReadEngineState,osTimerPeriodic);
00539  RtosTimer Task3(Task3_CalcAvSpeed,osTimerPeriodic);
00540  RtosTimer Task4(Task4_UpdateRCWiper,osTimerPeriodic);
00541  RtosTimer Task5(Task5_OverspeedLED,osTimerPeriodic);
00542  RtosTimer Task6(Task6_UpdateOdometer,osTimerPeriodic);
00543  RtosTimer Task7(Task7_SendToMailQueue,osTimerPeriodic);
00544  RtosTimer Task8(Task8_DumpSerial,osTimerPeriodic); 
00545  RtosTimer Task9(Task9_ReadSideLight,osTimerPeriodic);
00546  RtosTimer Task10(Task10_ReadIndicatorLights,osTimerPeriodic);
00547  
00548  // Staert RtosTimer objects, with the required frequency
00549  CarSim.start(50);      // 20Hz
00550  Task1.start(100);      // 10Hz
00551  Task2.start(500);      // 2Hz
00552  Task3.start(200);      // 5Hz
00553  Task4.start(1000);     // 1Hz
00554  Task5.start(2000);     // 0.5Hz
00555  Task6.start(500);      // 2Hz
00556  Task7.start(5000);     // 0.2Hz
00557  Task8.start(20000);    // 0.05Hz
00558  Task9.start(1000);     // 1Hz
00559  Task10.start(2000);    // 0.5Hz 
00560  
00561  // Ensure that RtosTimer runs within Infinite loop
00562  Thread::wait(osWaitForever);
00563     
00564     
00565 }