File content as of revision 9:c236eaaacf08:

/* #####################################################################
                               main.cpp
                               ---------
                
                     Embedded Software - Assignment 3
                     --------------------------------
 
 Written by:        Steven Kay
 
 Date:              March 2016
 
 Function:          This code operates to simulate the operation of a Car's
                    control system. It does so by taking external input for 
                    elements of a Car; Light Indicators, Engine State and Pedal values
                    and using a simple Car Simulation model, derives a speed
                    from which several tasks are included to act upon.
                    An average speed is generated at a given frequency 
                    and a total distance is estimated using this average speed.
                    Output comes in the form of LED's when specific indicators 
                    are true, and on a LCD screen and Servo Wiper to indicate speed.
                    Furthermore, a PC Connection is established and a data dump of
                    current Accelerometer, Brake and Average speed is presented
                    to the user every 20 seconds.
                    
                    This system is constructed using the MBED-RTOS and 
                    as such, each Task has an associated frequency, found in main, below.
                    Note that no priority is given to the tasks and no 
                    extra scheduler controls the synchronisation of tasks,
 
 ##################################################################### */
 
#include "mbed.h"
#include "rtos.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"

// ============================================================================
// Define Statements
// ============================================================================

// LCD Definitions
#define BACK_LIGHT_ON(INTERFACE) INTERFACE->write_bit(1,BL_BIT)
#define BACK_LIGHT_OFF(INTERFACE) INTERFACE->write_bit(0,BL_BIT)

// General Logical Assignments
#define HIGH 1
#define LOW 0

// Car Sim, Maximum Car Speed (mph)
#define MAX_SPEED 100

// ============================================================================
// MBED Pin Assignments
// ============================================================================

// System Inputs
// ----------------------------------------------------------------------------

// Analog Channels
AnalogIn Brake(p19);                        // Brake Pedal
AnalogIn Accelerometer(p20);                // Accelerator Pedal

// Digitial Channels
DigitalIn EngineState(p18);                 // Engine State Switch
DigitalIn LeftIndicator(p17);               // Left Indicator Switch
DigitalIn RightIndicator(p16);              // Right Indicator Switch
DigitalIn SideLightIndicator(p15);          // Side Light Indicator


// System Outputs
// ----------------------------------------------------------------------------

// LED Indicators (Steady State)
DigitalOut EngineStateInd(LED1);            // Engine State LED
DigitalOut SideLightInd(LED2);              // Side Light LED

// LED Indicators (Flashing)
PwmOut LeftLightInd(LED3);                  // Left Indicator LED
PwmOut RightLightInd(LED4);                 // Right Indicator LED
PwmOut OverSpeedInd(p22);                   // OverSpeed LED

// Servo Output
PwmOut AvSpeedWiper(p21);                   // Average Speed Wiper

// USB Connection to PC
Serial PCConn(USBTX,USBRX);                 // Connection to PC

// LCD Objects
MCP23017 *par_port;                         // Object pointing to Expander
WattBob_TextLCD *lcd;                       // LCD Connection

// ============================================================================
// Global Data Structure Declerations
// ============================================================================

// Raw Data Structure
// ----------------------------------------------------------------------------

// CarRawData  is a global memory area and contaims an instance of the Raw data 
// populated by Task1 and can be accessed through locking Mutex - rawDataMutex
typedef struct
{
    bool EngineState;
    float RawAccelerometer;
    float RawBraking;
} CarRawData;

// Create Mutex to control access to CarRawData instance
Mutex rawDataMutex;

// Create Instance of CarRawData
CarRawData rawData;

// Speed Data Structure
// ----------------------------------------------------------------------------

// CarSpeedData is a global memory area and contains an instance of the calculated
// raw speed values and the index to the next available element in the array to be 
// written to. rawSpeed is calculated by the CarSim and can be accessed through
// locking Mutex - SpeedMutex
typedef struct
{
    float rawSpeed[3];
    int counter;
} CarSpeedData;

// Create Mutex to control access to CarSpeedData instance
Mutex SpeedMutex;

// Create instance of CarSpeedData
CarSpeedData speedData;

// Filtered Data Structure
// ----------------------------------------------------------------------------

float AverageSpeed;
float totalDistance;

// Create Mutex to control access to FilteredData
Mutex filteredDataMutex;

// Mail Queue Structure
// ----------------------------------------------------------------------------

// PCDumpData is a global memory area which is populated by Taak7 and used as
// the structure within a MailQueue. Data contained is a copy from the current
// state of the Control System at a given instance of time.
typedef struct
{
    float currentAverageSpeed;
    float currentAccelerometer;
    float currentBraking;
} PCDumpData;

// Construct a 100 Element Mail Queue structure
Mail<PCDumpData,100> Memory_Dump;

// Define a Counter to trakc number of entries to Mail Queue
int MailQueueCounter;

// Create Mutex to control access to the MailQueueCounter variable
Mutex MailMutex;


// ============================================================================
// Car Simulation
// ============================================================================

// The CarSimulator Task updates the rawSpeed parameter at a frequenct of 20Hz
void CarSimulator(void const *arg)
{
    float newSpeed;
    
    // Load Shared resources into local variables within the Task
    // Shared Resources are; Accelerometer value, Braking value and Engine State
    rawDataMutex.lock();
    float currentAccelerometer = rawData.RawAccelerometer;
    float currentBrake = rawData.RawBraking;
    bool currentEngineState = rawData.EngineState;
    rawDataMutex.unlock();   

    // Run simple model which estimates the speed, as a fraction of the MAX SPEED
    // based on the percentage of either accelerator or brake pressed by the user
    // Further, newSpeed is set to 0 if the currentEngineState is equal to 0 (Engine off)
    newSpeed = currentAccelerometer*MAX_SPEED*(1-currentBrake)*currentEngineState;
    
    // Check Speed Counter's range, if out of bounds of array length, reset Counter
    // to 0
    // Data contained within Shared Resource therefore the SpeedMutex is used to control access
    SpeedMutex.lock();
    if(speedData.counter > 2)
    {
        speedData.counter = 0;
    }
    
    // Output a rawSpeed value to the next available index of rawSpeed[] and increment Counter
    speedData.rawSpeed[speedData.counter] = newSpeed;
    speedData.counter = speedData.counter++;
    SpeedMutex.unlock();
}

// ============================================================================
// Control System Tasks
// ============================================================================

// Task1_ReadRawData gathers external inputs and updates the rawData structure
// It operates at a frequency of 10Hz
void Task1_ReadRawData(void const *arg)
{
      // Lock Shared Resource - rawData
      // Update rawData elements directly from AnalogIn channel values
      // Unlock Shares Resource
      rawDataMutex.lock();
      rawData.RawBraking = Brake.read();
      rawData.RawAccelerometer = Accelerometer.read();
      rawDataMutex.unlock();
}


// Task2_ReadEngineState updates the rawData structure and operates ar a frequency of 2Hz
void Task2_ReadEngineState(void const *arg)
{
    // Get external input from DigitalIn Channel and store in local variable
    bool currentEngineState = EngineState.read();
    
    // Lock Shared Resource - rawData
    // Take a copy of the local variable currentEngineState and store into Global memory
    // Unlock Shared Resource
    rawDataMutex.lock();
    rawData.EngineState = currentEngineState;
    rawDataMutex.unlock();
    
    // Conduct logical check on local varialbe currentEngineState
    // if currentEngineState is HIGH, set EngineStateInd to HIGH
    // else set EngineStateInd LOW
    if(currentEngineState)
    {
        EngineStateInd = HIGH;
    }
    else
    {
        EngineStateInd = LOW;
    }
}
       
       
// Task3_CalcAvSpeed updates the AverageSpeed global varialbe and operates at a frequency of 5Hz    
void Task3_CalcAvSpeed(void const *arg)
{
    // Initialise local variable as 0.0
    float speedTotal = 0.0;
    
    // Lock Shared Resource - speedData
    // Calculate total from array of rawSpeed values and store locally
    // Unlock Shared Resource
    SpeedMutex.lock();
    for(int num = 0; num < 3; num++)
    {
        speedTotal = speedTotal + speedData.rawSpeed[num];
    }
    SpeedMutex.unlock();

    // Lock Shared Resource - AverageSpeed
    // Calculate average from local variable speedTotal and store result Globally into AverageSpeed
    // Unlock Shared Resource
    filteredDataMutex.lock();
    AverageSpeed = (speedTotal/3);
    filteredDataMutex.unlock();
}


// Task4_UpdateRCWiper takes the AverageSpeed global variable at a given time and outputs 
// a representation of this through a Servo. It operates at a frequenct of 1Hz
void Task4_UpdateRCWiper(void const *arg)
{
    // Lock Shared Resource - AverageSpeed
    // Take local copy of AverageSpeed
    // Unlock Shared Resource
    filteredDataMutex.lock();
    float currentAverageSpeed = AverageSpeed;
    filteredDataMutex.unlock();
    
    // Update Servo Position based upon the local copy of AverageSpeed
    
    // Servo must be controlled in the range of 1000us to 2000us
    // Thus a base value of 1000 is included, and as currentAverageSpeed cannot exceed 100,
    // This is simply scaled by 10, to give a maximum of 2000, which allows Servo to operate
    // Over full operational range
    AvSpeedWiper.pulsewidth_us(1000+(10*currentAverageSpeed));
}


// Task5_OverspeedLED takes the 0th Element of the rawSpeed global variable and computes
// a logical assessment to detect when the speed is greater than a preset of 70mph and 
// indicate results through an LED. It operates at a frequency of 0.5Hz
void Task5_OverspeedLED(void const *arg)
{
    // Lock Shared Resource - speedData
    // Take local copy of rawSpeed[0]
    // // Unlock Shares Resource
    SpeedMutex.lock();
    float currentInstSpeed = speedData.rawSpeed[speedData.counter];
    SpeedMutex.unlock();
    
    // Using local copy of rawSpeed[0], if this is above preset threshold of 70,
    // OverSpeedInd is set HIGH, else it is set LOW
    if(currentInstSpeed > 70.0)
    {
        OverSpeedInd = HIGH;
    }
    else
    {
        OverSpeedInd = LOW;
    }
}


// Task6_UpdateOdometer takes the AverageSpeed global variable and calculates the
// distance travelled by the Car over a known time increment (the delta of time between this task being ran)
// Once calculated, the distance is stored globally and also, in conjunction with AverageSpeed, displayed onto
// a LCD screen. It operates at a frequency of 2Hz
void Task6_UpdateOdometer(void const *arg)
{   
    // Lock Shared Varialbe - AverageSpeed
    // Take local copy of AverageSpeed
    // Unlock Shared Variable
    filteredDataMutex.lock();
    float currentAverageSpeed = AverageSpeed;
    filteredDataMutex.unlock();
    
    // Compute newTotalDistance from current TotalDistance and average speed
    
    // distance = oldDistance +(currentAverageSpeed*timeIncrement)
    // Note that timeIncrement (0.5 second) is converted from seconds to hours, 
    // To remain consistant with mph units
    
    // NOTE
    // totalDistance does not need to be protected by a Mutex as this is the only task which updates 
    // or uses the variable. It is global in order to keep track of a rolling total
    totalDistance = totalDistance + (currentAverageSpeed*(0.5/3600));
    
    // Output totalDistance and currentAverageSpeed to LCD
    lcd -> cls();
    lcd -> locate(0,0);
    lcd -> printf("Dist: %8.2f",totalDistance);
    lcd -> locate(1,0);
    lcd -> printf("Speed: %3.1f mph",currentAverageSpeed);
}


// Task7_SendToMailQueue takes global variables; AverageSpeed, RawAccelerometer and RawBraking and
// creates a structure from them, then puts an instanc of this structure onto a Mail Queue, incrementing
// a globally defined Counter as it does such. It operates with a frequency of 0.2Hz
void Task7_SendToMailQueue(void const *arg)
{
    // Lock Shared Resource - AverageSpeed
    // Take local copy of AverageSpeed
    // Unlock Shared Resource
    filteredDataMutex.lock();
    float currentAverageSpeed = AverageSpeed;
    filteredDataMutex.unlock();
    
    // Lock Shared Resource - rawData
    // Take local copy of RawAccelerometer and RawBraking
    // Unlock Shared Resource
    rawDataMutex.lock();
    float currentAccelerometer = rawData.RawAccelerometer;
    float currentBrake = rawData.RawBraking;
    rawDataMutex.unlock();  
    
    // Allocate Memory for instance of PCDumpData structure
    PCDumpData *currentPCDump = Memory_Dump.alloc();
    
    // Populate instance of PCDumpData with local copies of desired variables
    currentPCDump -> currentAverageSpeed = currentAverageSpeed;
    currentPCDump -> currentAccelerometer = currentAccelerometer;
    currentPCDump -> currentBraking = currentBrake;
    
    // Push instance of PCDumpData onto Mail Queue
    Memory_Dump.put(currentPCDump);  
    
    // Lock Shared Resource - MailQueueCounter
    // Increment MailQueueCounter
    // Unlock Shared Resource
    MailMutex.lock();
    MailQueueCounter++;
    MailMutex.unlock();  
}


// Task8_DumpSerial takes all of the instances of PCDumpData which have been pushed onto the Mail Queue
// removes them from the Mail Queue and dumps their values over a PC serial connection. Once complete, the
// counter which stores the number of elements in the Mail Queue is reset to 0. It operates at a frequency of 0.05Hz
void Task8_DumpSerial(void const *arg)
{
    // Lock Shared Resource - MailQueueCounter
    // Take local copy of MailQueueCounter
    // Unlock Shared Resource
    MailMutex.lock();
    int currentQueueCounter = MailQueueCounter;
    MailMutex.unlock();
    
    // Prompt State of Memory Dump
    PCConn.printf("Memory Dump\r\n");
    
    // For each instance of PCDumpData found in the Mail Queue, import the structure and store locally
    // Then print the contained values and free the element of the Mail Queue
    // Repeat for all indexes of the Mail Queue
    for(int num = 0; num < currentQueueCounter; num++)
    {
        osEvent evt = Memory_Dump.get();
        if(evt.status == osEventMail)
        {
            PCDumpData *currentPCDump = (PCDumpData*)evt.value.p;
            
            PCConn.printf("Av Speed: %f\r\nAcceler: %f\r\nBrake: %f\r\n\r\n",  currentPCDump -> currentAverageSpeed,
                                                                    currentPCDump -> currentAccelerometer,
                                                                    currentPCDump -> currentBraking);
            Memory_Dump.free(currentPCDump);
        }
    }
    
    // Lock Shared Resource - MailQueueCounter
    // Reset MailQueueCounter
    // Unlock Shared Resource
    MailMutex.lock();
    MailQueueCounter = 0;
    MailMutex.unlock();
}


// Task9_ReadSideLight takes external input from SideLightIndicator and conducts a logical test.
// It operates at a frequency of 1Hz
void Task9_ReadSideLight(void const *arg)
{
    // If SideLightIndicator is HIGH, set SideLightInd to HIGH, else set to LOW
    if(SideLightIndicator)
    {
        SideLightInd = HIGH;
    }
    else
    {
        SideLightInd = LOW;  
    }  
}


// Task10_ReadIndicatorLights takes external input from LeftIndicator and RightIndicator and conducts a
// logical test. It operates at a frequency of 0.5Hz
void Task10_ReadIndicatorLights(void const *arg)
{

    // If LeftIndicator Only is HIGH, flash LeftLightInd at a frequency of 1Hz, 50% Duty
    if(LeftIndicator && !RightIndicator)
    {
        LeftLightInd.period(1.0);
        LeftLightInd.pulsewidth(0.5);
        
        RightLightInd.period(1.0);
        RightLightInd.pulsewidth(0.0);
    }
    
    // If RightIndicator Only is HIGH, flash RightLightInd at a frequency of 1Hz, 50% Duty
    else if(!LeftIndicator && RightIndicator)
    {
        LeftLightInd.period(1.0);
        LeftLightInd.pulsewidth(0.0);
        
        RightLightInd.period(1.0);
        RightLightInd.pulsewidth(0.5);
    }
    
    // If LeftIndicator and RightIndicator  are HIGH, flash LeftLightInd and RightLightInd
    //  at a frequency of 2Hz, 50% Duty
    else if(LeftIndicator && RightIndicator)
    {
        LeftLightInd.period(0.5);
        RightLightInd.period(0.5);
        
        LeftLightInd.pulsewidth(0.25);
        RightLightInd.pulsewidth(0.25);
    }
    // Else, turn off both LeftLightInd and RightLightInd
    else
    {
        LeftLightInd.period(1.0);
        LeftLightInd.pulsewidth(0.0);
        
        RightLightInd.period(1.0);
        RightLightInd.pulsewidth(0.0);
    }
}
      
      
void InitSystem()
{
    // Set AvSpeedWiper to 50Hz frequency, for Servo
    AvSpeedWiper.period_ms(20);
    
    // Initiate LCD by lighting Backlight
    par_port->write_bit(1,BL_BIT);

    // Initiate all Global variables to 0
    rawData.EngineState = 0;
    rawData.RawAccelerometer = 0.0;
    rawData.RawBraking = 0.0;
    speedData.counter = 0;
    speedData.rawSpeed[0] = 0.0;
    speedData.rawSpeed[1] = 0.0;
    speedData.rawSpeed[2] = 0.0;
    AverageSpeed = 0.0;
    totalDistance = 0.0;
    MailQueueCounter = 0;
}
    

// ============================================================================
// Entry Point Thread
// ============================================================================

// Entry Point Thread, this shall be the initialiser for the Car System and 
// Contains all of the Tasks and their associated properties, such as frequency.
int main()
{
 // Construct Objects for LCD
 par_port = new MCP23017(p9, p10, 0x40); // initialise 16-bit I/O chip
 lcd = new WattBob_TextLCD(par_port); // initialise 2*26 char display

 // Set PC Connection Baud Rate
 PCConn.baud(115200);
 
 // Initialise System, including Global Variables
 InitSystem();
  
// Construct Tasks as RtosTimer objects
 RtosTimer CarSim(CarSimulator,osTimerPeriodic);
 RtosTimer Task1(Task1_ReadRawData,osTimerPeriodic);
 RtosTimer Task2(Task2_ReadEngineState,osTimerPeriodic);
 RtosTimer Task3(Task3_CalcAvSpeed,osTimerPeriodic);
 RtosTimer Task4(Task4_UpdateRCWiper,osTimerPeriodic);
 RtosTimer Task5(Task5_OverspeedLED,osTimerPeriodic);
 RtosTimer Task6(Task6_UpdateOdometer,osTimerPeriodic);
 RtosTimer Task7(Task7_SendToMailQueue,osTimerPeriodic);
 RtosTimer Task8(Task8_DumpSerial,osTimerPeriodic); 
 RtosTimer Task9(Task9_ReadSideLight,osTimerPeriodic);
 RtosTimer Task10(Task10_ReadIndicatorLights,osTimerPeriodic);
 
 // Staert RtosTimer objects, with the required frequency
 CarSim.start(50);      // 20Hz
 Task1.start(100);      // 10Hz
 Task2.start(500);      // 2Hz
 Task3.start(200);      // 5Hz
 Task4.start(1000);     // 1Hz
 Task5.start(2000);     // 0.5Hz
 Task6.start(500);      // 2Hz
 Task7.start(5000);     // 0.2Hz
 Task8.start(20000);    // 0.05Hz
 Task9.start(1000);     // 1Hz
 Task10.start(2000);    // 0.5Hz 
 
 // Ensure that RtosTimer runs within Infinite loop
 Thread::wait(osWaitForever);
    
    
}