File content as of revision 4:b4bcb329a930:

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

#define BACK_LIGHT_ON(INTERFACE) INTERFACE->write_bit(1,BL_BIT)

#define BACK_LIGHT_OFF(INTERFACE) INTERFACE->write_bit(0,BL_BIT)

MCP23017 *par_port;
WattBob_TextLCD *lcd;

#define TRUE 1
#define FALSE 0

#define HIGH 1
#define LOW 0

// ============================================================================
// MBED Pin Assignments
// ============================================================================
AnalogIn Brake(p19);
AnalogIn Accelerometer(p20);

DigitalIn EngineState(p18);
DigitalIn LeftIndicator(p17);
DigitalIn RightIndicator(p16);
DigitalIn SideLightIndicator(p15);

DigitalOut EngineStateInd(LED1);
DigitalOut SideLightInd(LED2);

PwmOut LeftLightInd(LED3);
PwmOut RightLightInd(LED4);
PwmOut OverSpeedInd(p22);

PwmOut AvSpeedWiper(p21);

Serial PCConn(USBTX,USBRX);


// ============================================================================
// Global Data Structure Declerations
// ============================================================================
typedef struct
{
    bool EngineState;
    float RawAccelerometer;
    float RawBraking;
} CarRawParams;

Mutex rawParamsMutex;
CarRawParams rawParams;


typedef struct
{
    float rawSpeed[4];
} CarProcessedParams;


Mutex lastCalculatedrawSpeedMutex;


typedef struct
{
    float AverageSpeed;
} CarFilteredParams;

Mutex filteredParamsMutex;
CarFilteredParams filteredParams;



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

void CarSimulator(void const *arg)
{
    PCConn.printf("CarSim\r\n");
    rawParamsMutex.lock();
    float currentAccelerometer = rawParams.RawAccelerometer;
//    PCConn.printf("%f\r\n",currentAccelerometer);
    float currentBrake = rawParams.RawBraking;
    bool currentEngineState = rawParams.EngineState;
    rawParamsMutex.unlock();
    
    lastCalculatedrawSpeedMutex.lock();
    float currentRawSpeed = lastCalculatedrawSpeed;
    lastCalculatedrawSpeedMutex.unlock();
    
    if(currentEngineState)
    {
        if(currentRawSpeed >= 100)
        {
            CarProcessedParams *pr\ocessedParams = mpool.alloc();
            processedParams -> rawSpeed = 100.0;
            AvSpeedQueue.put(processedParams);
            
            lastCalculatedrawSpeedMutex.lock();
            lastCalculatedrawSpeed = 100.0;
            lastCalculatedrawSpeedMutex.unlock();
        }
        else
        {
            // Calculate speed in m/s, convert to mph
            float newSpeed = (currentRawSpeed + ((7*(currentAccelerometer - currentBrake))*0.05));
            
            CarProcessedParams *processedParams = mpool.alloc();
            processedParams -> rawSpeed = newSpeed;
            AvSpeedQueue.put(processedParams);
            
            lastCalculatedrawSpeedMutex.lock();
            lastCalculatedrawSpeed = newSpeed;
            lastCalculatedrawSpeedMutex.unlock();
            
        }
    }
    // If Engine State == OFF, assume Car is stationary
    else
    {
        CarProcessedParams *processedParams = mpool.alloc();
        processedParams -> rawSpeed = 0.0;
        AvSpeedQueue.put(processedParams);
            
        lastCalculatedrawSpeedMutex.lock();
        lastCalculatedrawSpeed = 0.0;
        lastCalculatedrawSpeedMutex.unlock();
    }
    
    PCConn.printf("%f\r\n",lastCalculatedrawSpeed);
}


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

void Task1_ReadRawData(void const *arg)
{
      PCConn.printf("Task1\r\n");
      rawParamsMutex.lock();
      rawParams.RawBraking = Brake.read();
//      PCConn.printf("%f\r\n",rawParams.RawBraking);
      rawParams.RawAccelerometer = Accelerometer.read();
//      PCConn.printf("%f\r\n",rawParams.RawAccelerometer);
      rawParamsMutex.unlock();
}



void Task2_ReadEngineState(void const *arg)
{
    PCConn.printf("Task2\r\n");
    bool currentEngineState = EngineState.read();
    rawParamsMutex.lock();
    rawParams.EngineState = currentEngineState;
    rawParamsMutex.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++)
    {
        osEvent evt = AvSpeedQueue.get();
        if(evt.status == osEventMessage)
        {
            CarProcessedParams *processedParams = (CarProcessedParams*)evt.value.p;
            speedTotal = speedTotal + processedParams->rawSpeed;
            PCConn.printf("Total: %f\r\n",speedTotal);
            mpool.free(processedParams);
        }
    }

    filteredParamsMutex.lock();
    filteredParams.AverageSpeed = (speedTotal/3);
    filteredParamsMutex.unlock();
}

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



void Task5_OverspeedLED(void const *arg)
{
    PCConn.printf("Task5\r\n");
    float currentInstSpeed = 0.0;
    osEvent evt = AvSpeedQueue.get();
    
    if(evt.status == osEventMessage)
    {
        CarProcessedParams *processedParams = (CarProcessedParams*)evt.value.p;
        currentInstSpeed = processedParams->rawSpeed;
    }
    
    if(currentInstSpeed > 70.0)
    {
        OverSpeedInd = HIGH;
    }
    else
    {
        OverSpeedInd = LOW;
    }
    
}


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

    rawParams.EngineState = 0;
    rawParams.RawAccelerometer = 0.0;
    rawParams.RawBraking = 0.0;

    lastCalculatedrawSpeed = 0.0;
    
    filteredParams.AverageSpeed = 0.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 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
 
 
 Task9.start(1000); // 1Hz
 Task10.start(2000);// 0.5Hz 
 
 Thread::wait(osWaitForever);
    
    
}

//
//
//Mutex readvalues;
//Semaphore read_s(1);
//
//Mail<value_t, 100> mail_box;
//
//
//float readValue = 1.0;
//
//typedef struct
//{
//    float AccelerationRaw;
//    float BrakeRaw;
//} value_t;
//
//
//void readAnalogPins(void const *args)
//{
//    while(1)
//    {
//        value_t *ReadValue = mail_box.alloc();
//        
//        ReadValue -> AccelerationrRaw = Accelerometer.read();
//        ReadValue -> BrakeRaw = Brake.read();
//        
//        mail_box.put(ReadValue);
//        Thread::wait(100); 
//    }
//    
//}
//
//void sendToPC(void const *args)
//{
//    while(1)
//    {
//        osEvent evt = mail_box.get();
//        
//        if(evt.status == osEventMail)
//        {
//            value_t *ReadValue = (value_t*)evt.value.p;
//            printf("Value: %1.3f\r\n", ReadValue-> vale);
//            mail_box.free(ReadValue);
//        }
//        Thread::wait(5000); 
//    }
//    
//}
//
//int main() {
//    
//    Thread thread_1(readData);    
//    Thread thread_2(sendToPC);
//    
//    while(1) {
//        
//        myled = 1;
//        wait(0.2);
//        myled = 0;
//        wait(0.2);
//    }
//}