Diff: main.cpp

Revision:

5:4f2a072ed289

Parent:

4:b4bcb329a930

Child:

6:572b9755f2c1

diff -r b4bcb329a930 -r 4f2a072ed289 main.cpp
--- a/main.cpp	Wed Mar 30 02:23:10 2016 +0000
+++ b/main.cpp	Wed Mar 30 03:52:21 2016 +0000
@@ -55,22 +55,26 @@
 
 typedef struct
 {
-    float rawSpeed[4];
-} CarProcessedParams;
+    float rawSpeed[3];
+    int counter;
+} CarSpeed;
 
 
-Mutex lastCalculatedrawSpeedMutex;
+float lastSpeed;
+Mutex SpeedMutex;
+CarSpeed speedParams;
 
 
 typedef struct
 {
     float AverageSpeed;
+    float totalDistance;
 } CarFilteredParams;
 
 Mutex filteredParamsMutex;
 CarFilteredParams filteredParams;
 
-
+float totalDistance;
 
 // ============================================================================
 // Car Simulation
@@ -79,57 +83,44 @@
 void CarSimulator(void const *arg)
 {
     PCConn.printf("CarSim\r\n");
+    
+    float newSpeed;
+    
     rawParamsMutex.lock();
     float currentAccelerometer = rawParams.RawAccelerometer;
-//    PCConn.printf("%f\r\n",currentAccelerometer);
+    PCConn.printf("A: %f\r\n",currentAccelerometer);
     float currentBrake = rawParams.RawBraking;
+    PCConn.printf("B: %f\r\n",currentBrake);
     bool currentEngineState = rawParams.EngineState;
-    rawParamsMutex.unlock();
+    rawParamsMutex.unlock();   
     
-    lastCalculatedrawSpeedMutex.lock();
-    float currentRawSpeed = lastCalculatedrawSpeed;
-    lastCalculatedrawSpeedMutex.unlock();
+    SpeedMutex.unlock();
+    float lastSpeed_copy = lastSpeed;
+    SpeedMutex.lock();
+
+        // Calculate speed in m/s, convert to mph
+//        float Acc = ((lastSpeed_copy/2.23) + ((7*currentAccelerometer)*0.05));
+//        float Dec = ((lastSpeed_copy/2.23) + ((7*currentBrake)*0.05));
+//        
+//        newSpeed = (Acc - Dec)*2.23;
+#define MAX_SPEED 100
+    newSpeed = currentAccelerometer*MAX_SPEED*(1-currentBrake)*currentEngineState;
     
-    if(currentEngineState)
+    SpeedMutex.lock();
+    if(speedParams.counter > 2)
     {
-        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();
+        speedParams.counter = 0;
     }
     
-    PCConn.printf("%f\r\n",lastCalculatedrawSpeed);
+    speedParams.rawSpeed[speedParams.counter] = newSpeed;
+    SpeedMutex.unlock();
+    
+    lastSpeed = newSpeed;
+    
+    speedParams.counter = speedParams.counter++;
+    
+    
+    
 }
 
 
@@ -137,14 +128,13 @@
 // 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();
 }
 
@@ -167,28 +157,28 @@
         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);
-        }
+        speedTotal = speedTotal + speedParams.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");
@@ -196,7 +186,7 @@
     float currentAverageSpeed = filteredParams.AverageSpeed;
     filteredParamsMutex.unlock();
     
-    AvSpeedWiper.pulsewidth_us(2*currentAverageSpeed);
+    AvSpeedWiper.pulsewidth_us(1000+(10*currentAverageSpeed));
 }
 
 
@@ -204,14 +194,9 @@
 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;
-    }
+    SpeedMutex.lock();
+    float currentInstSpeed = speedParams.rawSpeed[speedParams.counter];
+    SpeedMutex.unlock();
     
     if(currentInstSpeed > 70.0)
     {
@@ -225,6 +210,39 @@
 }
 
 
+void Task6_UpdateOdometer(void const *arg)
+{
+    totalDistance = 50;
+    
+    filteredParamsMutex.lock();
+    float currentAverageSpeed = filteredParams.AverageSpeed;
+    filteredParamsMutex.unlock();
+    
+    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)
+{
+    
+    
+}
+
+
+
+void Task8_DumpSerial(void const *arg)
+{
+    
+    
+}
+
+
+
 void Task9_ReadSideLight(void const *arg)
 {
     PCConn.printf("Task9\r\n");
@@ -294,7 +312,10 @@
     rawParams.RawAccelerometer = 0.0;
     rawParams.RawBraking = 0.0;
 
-    lastCalculatedrawSpeed = 0.0;
+    speedParams.counter = 0;
+    speedParams.rawSpeed[0] = 0.0;
+    speedParams.rawSpeed[1] = 0.0;
+    speedParams.rawSpeed[2] = 0.0;
     
     filteredParams.AverageSpeed = 0.0;
 }
@@ -321,22 +342,23 @@
  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
- 
- 
- Task9.start(1000); // 1Hz
- Task10.start(2000);// 0.5Hz 
+ 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);