File content as of revision 8:6fad4bd89240:

/* #####################################################################
                               main.cpp
                               ---------
                
                     Embedded Software - Assignment 3
                     --------------------------------
 
 Written by:        Steven Kay
 
 Date:              March 2016
 
 Function:          This is the main runner containing the Cyclic executive
                    There are 7 defined tasks and several Auxillary components
                    which are logically ran periodically at their required time
                    by a Cyclic Executive sequencer.
                    Ticks, or slots, to this Cyclic Executive are provided by
                    a ticker ever 25ms, and then using logical expressions, the
                    correct task is initiated and allocated the required time.
 
 ##################################################################### */
 
#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
// ----------------------------------------------------------------------------
typedef struct
{
    bool EngineState;
    float RawAccelerometer;
    float RawBraking;
} CarRawData;

Mutex rawDataMutex;
CarRawData rawData;

// Speed Data Structure
// ----------------------------------------------------------------------------
typedef struct
{
    float rawSpeed[3];
    int counter;
} CarSpeedData;

float lastSpeed;

Mutex SpeedMutex;
CarSpeedData speedData;

// Filtered Data Structure
// ----------------------------------------------------------------------------
typedef struct
{
    float AverageSpeed;
} CarFilteredParams;

Mutex filteredParamsMutex;
CarFilteredParams filteredParams;

float totalDistance;

// Mail Queue Structure
// ----------------------------------------------------------------------------
typedef struct
{
    float currentAverageSpeed;
    float currentAccelerometer;
    float currentBraking;
} PCDump_t;

Mail<PCDump_t,100> Memory_Dump;

Mutex MailMutex;
int MailQueueCounter;


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

void CarSimulator(void const *arg)
{
    float newSpeed;
    
    rawDataMutex.lock();
    float currentAccelerometer = rawData.RawAccelerometer;
    float currentBrake = rawData.RawBraking;
    bool currentEngineState = rawData.EngineState;
    rawDataMutex.unlock();   

    newSpeed = currentAccelerometer*MAX_SPEED*(1-currentBrake)*currentEngineState;
    
    SpeedMutex.lock();
    if(speedData.counter > 2)
    {
        speedData.counter = 0;
    }
    
    speedData.rawSpeed[speedData.counter] = newSpeed;
    SpeedMutex.unlock();
        
    speedData.counter = speedData.counter++;
}

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


void Task1_ReadRawData(void const *arg)
{
//      PCConn.printf("Task1\r\n");
      rawDataMutex.lock();
      rawData.RawBraking = Brake.read();
      rawData.RawAccelerometer = Accelerometer.read();
      rawDataMutex.unlock();
}



void Task2_ReadEngineState(void const *arg)
{
//    PCConn.printf("Task2\r\n");
    bool currentEngineState = EngineState.read();
    rawDataMutex.lock();
    rawData.EngineState = currentEngineState;
    rawDataMutex.unlock();
    
    if(currentEngineState)
    {
        EngineStateInd = HIGH;
    }
    else
    {
        EngineStateInd = LOW;
    }
}
       
       
        
void Task3_CalcAvSpeed(void const *arg)
{
//    PCConn.printf("Task3\r\n");
    float speedTotal = 0.0;
    
    for(int num = 0; num < 3; num++)
    {
        speedTotal = speedTotal + speedData.rawSpeed[num];
//        PCConn.printf("Total: %f\r\n",speedTotal);
    }

    filteredParamsMutex.lock();
    filteredParams.AverageSpeed = (speedTotal/3);
//    PCConn.printf("Av: %f\r\n",filteredParams.AverageSpeed);
    filteredParamsMutex.unlock();
}



void Task4_UpdateRCWiper(void const *arg)
{
//    PCConn.printf("Task4\r\n");
    filteredParamsMutex.lock();
    float currentAverageSpeed = filteredParams.AverageSpeed;
    filteredParamsMutex.unlock();
    
    AvSpeedWiper.pulsewidth_us(1000+(10*currentAverageSpeed));
}



void Task5_OverspeedLED(void const *arg)
{
//    PCConn.printf("Task5\r\n");
    SpeedMutex.lock();
    float currentInstSpeed = speedData.rawSpeed[speedData.counter];
    SpeedMutex.unlock();
    
    if(currentInstSpeed > 70.0)
    {
        OverSpeedInd = HIGH;
    }
    else
    {
        OverSpeedInd = LOW;
    }
    
}


void Task6_UpdateOdometer(void const *arg)
{   
    filteredParamsMutex.lock();
    float currentAverageSpeed = filteredParams.AverageSpeed;
    filteredParamsMutex.unlock();
    
    totalDistance = totalDistance + (currentAverageSpeed*(0.5/3600));
    
    lcd -> cls();
    lcd -> locate(0,0);
    lcd -> printf("Dist: %8.2f",totalDistance);
    lcd -> locate(1,0);
    lcd -> printf("Speed: %3.1f mph",currentAverageSpeed);
}


void Task7_SendToMailQueue(void const *arg)
{
    filteredParamsMutex.lock();
    float currentAverageSpeed = filteredParams.AverageSpeed;
    filteredParamsMutex.unlock();
    
    rawDataMutex.lock();
    float currentAccelerometer = rawData.RawAccelerometer;
    float currentBrake = rawData.RawBraking;
    rawDataMutex.unlock();  
    
    PCDump_t *currentPCDump = Memory_Dump.alloc();
    currentPCDump -> currentAverageSpeed = currentAverageSpeed;
    currentPCDump -> currentAccelerometer = currentAccelerometer;
    currentPCDump -> currentBraking = currentBrake;
    
    Memory_Dump.put(currentPCDump);  
    
    MailMutex.lock();
    MailQueueCounter++;
    MailMutex.unlock();  
}



void Task8_DumpSerial(void const *arg)
{
    MailMutex.lock();
    int currentQueueCounter = MailQueueCounter;
    MailMutex.unlock();
    
    PCConn.printf("Memory Dump\r\n");
    
    for(int num = 0; num < currentQueueCounter; num++)
    {
        osEvent evt = Memory_Dump.get();
        if(evt.status == osEventMail)
        {
            PCDump_t *currentPCDump = (PCDump_t*)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);
        }
    }
    
    MailMutex.lock();
    MailQueueCounter = 0;
    MailMutex.unlock();
}



void Task9_ReadSideLight(void const *arg)
{
//    PCConn.printf("Task9\r\n");
    if(SideLightIndicator)
    {
        SideLightInd = HIGH;
    }
    else
    {
        SideLightInd = LOW;  
    }  
}



void Task10_ReadIndicatorLights(void const *arg)
{
//    PCConn.printf("Task10\r\n");
    // Left Indicator Only
    if(LeftIndicator && !RightIndicator)
    {
        LeftLightInd.period(1.0);
        LeftLightInd.pulsewidth(0.5);
        
        RightLightInd.period(1.0);
        RightLightInd.pulsewidth(0.0);
    }
    
    // Right Indicator Only
    else if(!LeftIndicator && RightIndicator)
    {
        LeftLightInd.period(1.0);
        LeftLightInd.pulsewidth(0.0);
        
        RightLightInd.period(1.0);
        RightLightInd.pulsewidth(0.5);
    }
    
    // Left and Right Indicators
    else if(LeftIndicator && RightIndicator)
    {
        LeftLightInd.period(0.5);
        RightLightInd.period(0.5);
        
        LeftLightInd.pulsewidth(0.25);
        RightLightInd.pulsewidth(0.25);
    }
    
    else
    {
        LeftLightInd.period(1.0);
        LeftLightInd.pulsewidth(0.0);
        
        RightLightInd.period(1.0);
        RightLightInd.pulsewidth(0.0);
    }
}
      
      
void InitSystem()
{
    AvSpeedWiper.period_ms(20);
    
    par_port->write_bit(1,BL_BIT); // turn LCD backlight ON

    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;
    
    filteredParams.AverageSpeed = 0.0;
    
    totalDistance = 0.0;

    MailQueueCounter = 0;
}
    
     
      
// ============================================================================
// Entry Point Thread
// ============================================================================

int main()
{
 par_port = new MCP23017(p9, p10, 0x40); // initialise 16-bit I/O chip
 lcd = new WattBob_TextLCD(par_port); // initialise 2*26 char display

 PCConn.baud(115200);
 
 InitSystem();
  
 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);
 
 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 
 
 Thread::wait(osWaitForever);
    
    
}