File content as of revision 20:aeb7b7044c31:

// Frazer Legge - H001262392
// Embedded Software - Assignment 3
// 
// This program operates a simulation of a car control system.
// THe simulation is completed through the use of external controls,
// ignition, sidelights switch, idicator switches, accelerator and 
// brake pedals. 
// The simulation calculates Average speed and total distance travelled.
// The simulation outputs are onboard LEDs to show ignition, sidelights,
// indicators and overspeed indicator. The system shows the distance 
// traveled and average speed of the car on the LCD. A USB dump is also 
// done showing the average speed, accelerator and brake positions.


#include "mbed.h"
#include "rtos.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"


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

// Car Sim Max/Min speed (mph)
#define MAX_SPEED 100
#define MIN_SPEED 0

// Serial Connection
Serial pc(USBTX, USBRX,115200);

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

// System Inputs
// ----------------------------------------------------------------------------
// Analogue Input
AnalogIn Accel(p15);            // Accelerator Pedal Input
AnalogIn Brake(p16);            // Brake Pedal Input

// Digital Input
DigitalIn Ignit(p11);           // Engine On/Off
DigitalIn SideL(p12);           // Side Light On/Off
DigitalIn Indi_L(p13);          // Left indicator switch
DigitalIn Indi_R(p14);          // Right idicator switch

// System Outputs
// ----------------------------------------------------------------------------
// PWM Output
PwmOut SpeedIndicator(p21);     // Speed Indicator Servo
PwmOut Indi_L_LED(LED3);        // Left Indicator LED
PwmOut Indi_R_LED(LED4);        // Right Indicator LED

// Digital Output
DigitalOut Ignit_LED(LED1);     // Ignition LED Indicator
DigitalOut SideL_LED(LED2);     // Side Light Indicator
DigitalOut OverS_LED(p17);        // Overspeed LED (REDBOX)

// Pointers to LCD Screen
MCP23017            *par_port;  // pointer to 16-bit parallel I/O chip
WattBob_TextLCD     *lcd;       // pointer to 2*16 character LCD object

// ============================================================================
// Data Structure 
// ============================================================================
//

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

// RawData is a global memory section that contains an instance of the raw input
// data from speedControl.

typedef struct
{
    bool EngineState;
    float RawAccel;
    float RawBrake;
} RawData;

// Create MUTEX to contol access to RawData 
Mutex RawDataMutex;

// Create instance of RawData
RawData rawData;

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

// Speed data is a global memory section that contains an instance of the 
// calculated car speed.  

typedef struct
{
    float rawSpeed[3];
    int counter;
} CarSpeedData;

// Create MUTEX to control access to CarSpeedData

Mutex SpeedDataMutex;

// Create insance of CarSpeedData
CarSpeedData speedData;

// Filtered Data Structure
// ----------------------------------------------------------------------------
typedef struct
{
    float AvgSpeed; 
    float TotalDist;
} FilteredData;

// Create MUTEX to control access to FilterData
Mutex filteredDataMutex;

// Create instance of FilteredData
FilteredData filteredData;

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

// PCDump is a global memory section that is populated in Mail_queue. The data 
// is coppied from the current state of the control system every 5 seconds.

typedef struct 
{
    float   speed;
    float   accelerator;
    float   brake;
} PCDump;

// Create 100 element Mail Queue
Mail<PCDump, 100> Memory_dump;

// Counter to track number of elements in mail
int MailCounter;

// Create MUTEX to control access to MailQueueCounter
Mutex MailMutex;
 
// ============================================================================
// Car Simulation
// ============================================================================ 
 
// The CarSim task updates the rawSpeed float at freq of 20Hz

void CarSim(void const *arg) 
{ 
    float NewSpeed;
    
    // Deposit global variables to local variables
    RawDataMutex.lock();
    bool currentEngineState = rawData.EngineState;
    float currentAccel = rawData.RawAccel;
    float currentBrake = rawData.RawBrake;
    RawDataMutex.unlock();
    
    // Calulate current speed as an fraction of the MAX_SPEED based on the 
    // percentage of accelerator and brake. 
    // Speed is set to 0 if Engine State is changed to 0;
    NewSpeed = currentAccel*MAX_SPEED*(1-currentBrake)*currentEngineState;
    
    // Check Counter. If outside array set to 0
    // Data within shared resource therefore MUTEX is used.
    SpeedDataMutex.lock();
    
    if (speedData.counter > 2)
    {
        speedData.counter = 0; 
    }
    
    // Output rawSpeed value to next index of rawSpeed. 
    // Increment counter
    speedData.rawSpeed[speedData.counter] = NewSpeed;
    speedData.counter++;
    SpeedDataMutex.unlock();
}
    
// ============================================================================
// Controller Tasks
// ============================================================================    

// Speed Control (Task 1) reads external analog inputs and updates rawData.
// Operating freq of 10Hz

void SpeedControl(void const *arg) 
{ 
    // Deposit global variables to local variables
    RawDataMutex.lock();
    rawData.RawAccel = Accel.read();
    rawData.RawBrake = Brake.read();
    RawDataMutex.unlock();
}

// Ignition(Task 2) reads the state of an "ignition switch". 
// Updates to rawData.
// Operating freq of 2Hz.

void Ignition(void const *arg) 
{
    // Get Switch state and store locally
    bool currentEngineState = Ignit.read();
    
    // Deposit Local data to global under locked state
    RawDataMutex.lock();
    rawData.EngineState = currentEngineState;
    RawDataMutex.unlock();
    
    // Logic check on Engine state
    if (currentEngineState == 1)
    { 
        Ignit_LED = 1;
    }
    else
    { 
        Ignit_LED = 0;
    }
}

// Speed_Avg (Task 3) Calculates the average speed of the car over 3 speed 
// samples.
// Operation freq of 5Hz

void Speed_Avg(void const *arg) 
{ 
    // Init local float as 0
    float speed_T = 0.0;
    
    // Deposit gloabal data to local data under locked state
    SpeedDataMutex.lock();
    for (int i = 0; i < 3; i++)
    { 
        speed_T = speed_T + speedData.rawSpeed[i];
    } 
    SpeedDataMutex.unlock();
    
    // Deposit local data to global data under locked state
    filteredDataMutex.lock();
    filteredData.AvgSpeed = (speed_T/3);
    filteredDataMutex.unlock();
}

// Speed_Indi (Task 4) shows a representation of the car speed through a servo
// Operation freq of 1Hz

void Speed_Indi(void const *arg) 
{
    // Deposit gloabal data to local data under locked state
    filteredDataMutex.lock();
    float currentAvgSpeed = filteredData.AvgSpeed;
    filteredDataMutex.unlock();
    
    // Update Servo
    // Operates between 1000us and 2000us pwm
    // ie -> 10 * 50(mph) will give middle positioning
    SpeedIndicator.pulsewidth_us(1000+(10*currentAvgSpeed));
}
 
// OverSpeed (Task 5) illuminates an LED indicator to show that the car is above
// 70mph.
// Opperation freq of 0.5Hz

void OverSpeed(void const *arg) 
{ 
    // Deposit gloabal data to local data under locked state
    SpeedDataMutex.lock();
    float currentSpeed = speedData.rawSpeed[1];
    SpeedDataMutex.unlock();
    
    // Use locally stored current speed to show if car is travelling over 70mph
    if (currentSpeed > 70.0)
    {
        OverS_LED = 1;
    }
    else
    { 
        OverS_LED = 0;
    }
} 

// Display (Taks 6) shows the total distance traveled and the average speed on 
// the LCD.
// Operation freq of 2Hz

void Display(void const *arg) 
{ 
    // Local distance variable
    float currentTotalDist;
    
    // Deposit gloabal data to local data under locked state
    filteredDataMutex.lock();
    float currentAvgSpeed = filteredData.AvgSpeed;
    float current_TotalDist = filteredData.TotalDist;
    filteredDataMutex.unlock();
    
    // Update distance traveled
    // Polling every 0.5 seconds therefore convereted from 0.5s to hours
    // MUTEX not protected as TotalDist is only used for this function 
    currentTotalDist = current_TotalDist + (currentAvgSpeed*(0.5/3600)); 
    
    filteredDataMutex.lock();
    filteredData.TotalDist = currentTotalDist;
    filteredDataMutex.unlock();
    
    // Output on LCD
    lcd->locate(0,0);
    lcd->printf("D: %f",currentTotalDist);
    lcd->locate(1,0); 
    lcd->printf("S: %f",currentAvgSpeed);
}

// Mail_Queue (Task 7) sends gloabal variable to a MAIL structure. 
// A counter is incremented to track the variables. 
// Operation freq of 0.2Hz.

void Mail_Queue(void const *arg) 
{ 
    // Deposit gloabal data to local data under locked state
    RawDataMutex.lock();
    float currentAccel = rawData.RawAccel;
    float currentBrake = rawData.RawBrake;
    RawDataMutex.unlock();
    
    filteredDataMutex.lock();
    float currentAvgSpeed = filteredData.AvgSpeed;
    filteredDataMutex.lock();

    // Allocate and populate PCDump Struct
    PCDump *currentPCDump = Memory_dump.alloc();
    
    currentPCDump->brake = currentBrake;
    currentPCDump->accelerator = currentAccel;
    currentPCDump->speed = currentAvgSpeed; 
    
    // PCDump struct pushed to mail queue
    Memory_dump.put(currentPCDump);
    
    // Lock mail MUTEX and increment counter
    MailMutex.lock();
    MailCounter++;
    MailMutex.unlock();
} 

// Mail_Print (Task 8) sends the MAIL queue to a terminal over USB serial. 
// When finished MAIL counter is reset.
// Operation freq of 0.05Hz.

void Mail_Print(void const *arg) 
{ 
    // Deposit gloabal data to local data under locked state
    MailMutex.lock();
    int currentMCount = MailCounter;
    MailMutex.unlock();
    
    // Terminal header notifying dump
    pc.printf("Mail Dump\n\r"); 
    
    // Print each record in MAIL queue. MAIL queue is deposited locally then 
    // values are printed and elements freed in MAIL queue. 
    for (int i = 0; i < currentMCount; i++)
    {
        osEvent evnt = Memory_dump.get();
        if (evnt.status == osEventMail)
        {
            PCDump *currentPCDump = (PCDump*)evnt.value.p;
            
            pc.printf("AvgSpeed: %f\n\rAccel: %f\n\rBrake: %f\n\rCounter: %i\n\n\r",currentPCDump->speed,currentPCDump->accelerator,currentPCDump->brake,i);
            
            Memory_dump.free(currentPCDump);
        }
    }
    
    // Reset counter under locked state
    MailMutex.lock();
    MailCounter = 0;
    MailMutex.unlock();
}

// Side_Light (Task 9) checks the state of the side light and sets the LED
// indicator
// Operation freq of 1Hz

void Side_Light(void const *arg) 
{ 
    if(SideL == 1)
    { 
        SideL_LED = 1;
    } 
    else 
    {
        SideL_LED = 0;
    } 
} 

// Indicator (Task 10) checks the left and right indicator switches.
// Illuminates either left or right LED accordingly.
// If both high then LEDs flash in hazard mode.

void Indicator(void const *arg) 
{ 
    // If Left switch is high then flash left LED at 1Hz
    if (Indi_L == 1 && Indi_R == 0)
    { 
        Indi_L_LED.period(1.0);
        Indi_L_LED.pulsewidth(0.5);
        
        Indi_R_LED.period(1.0);
        Indi_R_LED.pulsewidth(0.0);
    }
    
    // If Right switch is high then flash right LED at 1Hz
    else if (Indi_L == 0 && Indi_R == 1)
    { 
        Indi_L_LED.period(1.0);
        Indi_L_LED.pulsewidth(0.0);
        
        Indi_R_LED.period(1.0);
        Indi_R_LED.pulsewidth(0.5);
    }
    
    // If Left & Right switch is high then flash both LEDs at 2Hz
    else if (Indi_L == 1 && Indi_R == 1)
    { 
        Indi_L_LED.period(0.5);
        Indi_R_LED.period(0.5);
        Indi_L_LED.pulsewidth(0.25);
        Indi_R_LED.pulsewidth(0.25);
    }
    
    // If Left & Right switch is low then no flash
    else if (Indi_L == 0 && Indi_R == 0)
    { 
        Indi_L_LED.period(1.0);
        Indi_L_LED.pulsewidth(0.0);
        
        Indi_R_LED.period(1.0);
        Indi_R_LED.pulsewidth(0.0);
    }
}

void ledFlasher(void const *arg)
{
    Ignit_LED =! Ignit_LED;
}


// ============================================================================
// Init System
// ============================================================================ 
    
void Init()
{
    // Servo freq set
    SpeedIndicator.period_ms(50);
    // Global Variables zeroed
    rawData.EngineState = 0;
    rawData.RawAccel = 0.0;
    rawData.RawBrake = 0.0;
    speedData.rawSpeed[0] = 0.0;
    speedData.rawSpeed[1] = 0.0;
    speedData.rawSpeed[2] = 0.0;
    speedData.counter = 0;
    filteredData.AvgSpeed = 0.0;
    filteredData.TotalDist = 0.0;
    MailCounter = 0;
}

// ============================================================================
// Main Thread
// ============================================================================

int main()
{ 
    par_port = new MCP23017(p9, p10, 0x40); // initialise 16-bit I/O chip
    par_port->config(0x0F00, 0x0F00, 0x0F00);

    lcd = new WattBob_TextLCD(par_port);    // initialise 2*26 char display
    par_port->write_bit(1,BL_BIT);          // turn LCD backlight ON

    lcd->cls();                             // clear display
     
    Init();                                 // Run init void
    
    Ignit_LED = 0; 
    
    
    //while(1)
    //{
    //    Ignit_LED =! Ignit_LED;
    //    wait(1);
        //Ignit_LED = 0;
        //wait(1);
        
    //}
    
    //SideL_LED = 1;
    
    RtosTimer ledFlash(ledFlasher, osTimerPeriodic);
    ledFlash.start(1000);
    // Timer Objects
    //RtosTimer Car_Sim(CarSim,osTimerPeriodic);
    //RtosTimer Task1(SpeedControl,osTimerPeriodic);
    //RtosTimer Task2(Ignition,osTimerPeriodic);
    //RtosTimer Task3(Speed_Avg,osTimerPeriodic);
    //RtosTimer Task4(Speed_Indi,osTimerPeriodic);
    //RtosTimer Task5(OverSpeed,osTimerPeriodic);
    //RtosTimer Task6(Display,osTimerPeriodic);
    //RtosTimer Task7(Mail_Queue,osTimerPeriodic);
    //RtosTimer Task8(Mail_Print,osTimerPeriodic);
    //RtosTimer Task9(Side_Light,osTimerPeriodic);
    //RtosTimer Task10(Indicator,osTimerPeriodic);
    
    // Start RTOS Timer objects

    //Car_Sim.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);                     // 2Hz

    Thread::wait(osWaitForever);            // Forever loop
       
}