Diff: main.cpp

Revision:

4:b4bcb329a930

Parent:

3:8192bbde17b3

Child:

5:4f2a072ed289

--- a/main.cpp	Tue Mar 29 21:41:55 2016 +0000
+++ b/main.cpp	Wed Mar 30 02:23:10 2016 +0000
@@ -1,5 +1,14 @@
 #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
@@ -27,6 +36,9 @@
 
 PwmOut AvSpeedWiper(p21);
 
+Serial PCConn(USBTX,USBRX);
+
+
 // ============================================================================
 // Global Data Structure Declerations
 // ============================================================================
@@ -43,12 +55,11 @@
 
 typedef struct
 {
-    float rawAcceleration;
-    float rawSpeed;
+    float rawSpeed[4];
 } CarProcessedParams;
 
-Mutex processedParamsMutex;
-CarProcessedParams processedParams;
+
+Mutex lastCalculatedrawSpeedMutex;
 
 
 typedef struct
@@ -60,35 +71,65 @@
 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();
     
-    processedParamsMutex.lock();
-    float currentRawSpeed = processedParams.rawSpeed;
-    processedParamsMutex.unlock();
+    lastCalculatedrawSpeedMutex.lock();
+    float currentRawSpeed = lastCalculatedrawSpeed;
+    lastCalculatedrawSpeedMutex.unlock();
     
     if(currentEngineState)
     {
         if(currentRawSpeed >= 100)
         {
-            processedParamsMutex.lock();
-            processedParams.rawSpeed = 100;
-            processedParamsMutex.unlock();
+            CarProcessedParams *pr\ocessedParams = mpool.alloc();
+            processedParams -> rawSpeed = 100.0;
+            AvSpeedQueue.put(processedParams);
+            
+            lastCalculatedrawSpeedMutex.lock();
+            lastCalculatedrawSpeed = 100.0;
+            lastCalculatedrawSpeedMutex.unlock();
         }
         else
         {
-            // Car Sim Model
+            // 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);
 }
 
 
@@ -98,9 +139,12 @@
 
 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();
 }
 
@@ -108,8 +152,10 @@
 
 void Task2_ReadEngineState(void const *arg)
 {
+    PCConn.printf("Task2\r\n");
+    bool currentEngineState = EngineState.read();
     rawParamsMutex.lock();
-    bool currentEngineState = rawParams.EngineState;
+    rawParams.EngineState = currentEngineState;
     rawParamsMutex.unlock();
     
     if(currentEngineState)
@@ -122,35 +168,81 @@
     }
 }
         
-
+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);
+        }
+    }
 
-void Task3_UpdateRCWiper(void const *arg)
+    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_ms(2*currentAverageSpeed);
+    AvSpeedWiper.pulsewidth_us(2*currentAverageSpeed);
 }
 
 
 
-void Task8_ReadSideLight(void const *arg)
+void Task5_OverspeedLED(void const *arg)
 {
-    if(SideLightIndicator)
+    PCConn.printf("Task5\r\n");
+    float currentInstSpeed = 0.0;
+    osEvent evt = AvSpeedQueue.get();
+    
+    if(evt.status == osEventMessage)
     {
-        SideLightInd = TRUE;
+        CarProcessedParams *processedParams = (CarProcessedParams*)evt.value.p;
+        currentInstSpeed = processedParams->rawSpeed;
+    }
+    
+    if(currentInstSpeed > 70.0)
+    {
+        OverSpeedInd = HIGH;
     }
     else
     {
-        SideLightInd = FALSE;  
+        OverSpeedInd = LOW;
+    }
+    
+}
+
+
+void Task9_ReadSideLight(void const *arg)
+{
+    PCConn.printf("Task9\r\n");
+    if(SideLightIndicator)
+    {
+        SideLightInd = HIGH;
+    }
+    else
+    {
+        SideLightInd = LOW;  
     }  
 }
 
 
 
-void Task9_ReadIndicatorLights(void const *arg)
+void Task10_ReadIndicatorLights(void const *arg)
 {
+    PCConn.printf("Task10\r\n");
     // Left Indicator Only
     if(LeftIndicator && !RightIndicator)
     {
@@ -194,22 +286,18 @@
       
 void InitSystem()
 {
-     AvSpeedWiper.period_ms(20);
-
-
-typedef struct
-{
-    float AverageSpeed;
-} CarFilteredParams;
+    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;
-    
-    processedParams.rawAcceleration = 0.0;
-    processedParams.rawSpeed = 0.0;
+
+    lastCalculatedrawSpeed = 0.0;
     
     filteredParams.AverageSpeed = 0.0;
+}
     
      
       
@@ -219,25 +307,36 @@
 
 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); Thread::wait(2);
+ RtosTimer CarSim(CarSimulator,osTimerPeriodic);
  
- RtosTimer Task1(Task1_ReadRawData,osTimerPeriodic); Thread::wait(2);
- RtosTimer Task2(Task2_ReadEngineState,osTimerPeriodic); Thread::wait(2);
- RtosTimer Task3(Task3_UpdateRCWiper,osTimerPeriodic); Thread::wait(2);
- RtosTimer Task8(Task8_ReadSideLight,osTimerPeriodic); Thread::wait(2);
- RtosTimer Task9(Task9_ReadIndicatorLights,osTimerPeriodic); Thread::wait(2);
+ 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);
  
- CarSim.start(50);
- 
- Task1.start(100);
- Task2.start(500);
- Task3.start(1000);
  
  
- Task8.start(1000);
- Task9.start(2000);
+ 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);