Diff: source/my_tasks.cpp

Revision:

0:8940db3353d7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/my_tasks.cpp	Wed Mar 28 18:51:55 2018 +0000
@@ -0,0 +1,383 @@
+#include "my_tasks.h"
+#include "my_structures.h"
+#include "my_tools.h"
+
+#ifndef __CAR_STATS__
+#define __CAT_STATS__
+
+#define AIR_DENSITY         1.225   // kg/m3
+
+// TESLA MODEL X
+#define DRAG_COEF           0.24
+#define REF_AREA            0.62   // m²
+
+#endif
+
+using namespace std;
+
+// TASKS at 10Hz and 5Hz
+void timer1(void *arg)
+{
+    static int counterTimer1 = 0;
+    
+    counterTimer1++;
+    task1_readAccelBrake(arg);
+    
+    if (counterTimer1 % 2 == 0)
+    {
+        task3_filterSpeed(arg);
+    }
+}
+
+// TASKS at 2Hz, 1 Hz and 0.5 Hz
+void timer2(void *arg)
+{
+    static int counterTimer2 = 0;
+    
+    counterTimer2++;
+    task2_readEngineState(arg);
+    task4_showUseOfBrake(arg);
+    task6_writeLCD(arg);
+    
+    if (counterTimer2 % 2 == 0)
+    {
+        task5_monitorSpeed(arg);
+        task9_readSideLight(arg);
+    }
+    if (counterTimer2 % 4 == 0)
+    {
+        task10_turnIndic(arg);
+    }
+}
+
+// TASKS at 0.2 Hz and 0.05 Hz
+void timer3(void *arg)
+{
+    static int counterTimer3 = 0;
+    
+    counterTimer3++;
+    task7_sendToMailQueue(arg);
+    
+    if (counterTimer3 % 4 == 0)
+    {
+        task8_dumpMailQueue(arg);
+    }
+}
+
+
+
+
+
+
+
+
+
+// CAR SIMULATOR
+void task0_carSim(void *arg)
+{
+    const float dt = 0.05; // 20Hz
+    carStructure *myCar = (carStructure *) arg;
+    
+    // Block access on RAWValues structure
+    myCar->p_rawMutex->lock();
+    engineRAWValues &p_rawValues = *(myCar->p_rawValues);
+    
+    // Copy the values
+    int  engine = p_rawValues.engineState;
+    float accel = p_rawValues.acceleratorValue;
+    float brake = p_rawValues.brakeValue;
+    
+    // Release the mutex once we copied the values
+    myCar->p_rawMutex->unlock();
+    
+
+    
+    // Add it the vector saving the speed values
+    myCar->p_statMutex->lock();
+    // Get last speed value stored
+    float lastSpeed =(*(myCar->p_carStats->p_speedVector))[SIZE_SPEED_VECTOR-1];
+    myCar->p_statMutex->unlock();
+
+    // ---------------------------------------------------------------------- //
+    // Acceleration Force
+    float accelForce = MAX_SPEED * (accel - brake) * engine;
+    
+    // ---------------------------------------------------------------------- //
+    /* 
+     * DRAG FORCE -> F = 0.5 C ρ A V²
+     * 
+     * A = Reference area, m2.
+     * C = Drag coefficient, unitless.
+     * F = Drag force, N.
+     * V = Velocity, m/s.
+     * ρ = Density of fluid (liquid or gas), kg/m3.
+     */
+    float dragForce = 0.5*DRAG_COEF
+                        *lastSpeed*lastSpeed
+                        *REF_AREA
+                        *AIR_DENSITY;
+    if (lastSpeed == 0) dragForce = 0;
+
+    // ---------------------------------------------------------------------- //
+    // Compute speed of car on Euler Scheme
+    float newSpeed = lastSpeed*0.98
+                    + dt*(accelForce - dragForce);
+    if (newSpeed < 0) newSpeed = 0;
+
+    // ---------------------------------------------------------------------- //
+    // Lock Mutex
+    myCar->p_statMutex->lock();
+    // Delete first element of vector
+    deleteFirstValueVec(*(myCar->p_carStats->p_speedVector));
+    // Add new speed value
+    myCar->p_carStats->p_speedVector->push_back(newSpeed); // m/s
+    
+    // ---------------------------------------------------------------------- //
+    // Euler Scheme for traveled distance
+    float distance = myCar->p_carStats->distance + dt*newSpeed; //m
+    myCar->p_carStats->distance = distance;
+    // Unlock Mutex
+    myCar->p_statMutex->unlock();
+    
+    // ---------------------------------------------------------------------- //
+    myCar->p_PC->printf("Accel: %.2f | Brake: %.2f\r\n", accel, brake);
+    myCar->p_PC->printf("New Speed: %.1f km/h\r\n", newSpeed*3.6);
+    myCar->p_PC->printf("Traveled Distance: %.3f km\r\n", distance/1000);
+}
+
+// TASK 1
+void task1_readAccelBrake(void *arg)
+{
+    carStructure &myCar = *((carStructure *) arg);
+    
+    // Block access on RAWValues structure
+    (myCar.p_rawMutex)->lock();
+    engineRAWValues &p_rawValues = *(myCar.p_rawValues);
+    
+    // Read Accelerator and Brake Values
+    p_rawValues.acceleratorValue = readAnalogInput(*(myCar.p_accelerator));
+    p_rawValues.brakeValue       = readAnalogInput(*(myCar.p_brake));
+    
+    // Release the mutex
+    (myCar.p_rawMutex)->unlock();
+}
+
+// TASK 2                  
+void task2_readEngineState(void *arg)
+{
+    carStructure *myCar = (carStructure *) arg;
+    
+    // Block access on RAWValues structure
+    myCar->p_rawMutex->lock();
+    engineRAWValues &p_rawValues = *(myCar->p_rawValues);
+    
+    // Read Engine State
+    int state = myCar->p_engineSwitch->read();
+    p_rawValues.engineState = state;
+    if (state)
+    {
+        *(myCar->p_engineLight) = ON;
+    }
+    else
+    {
+        *(myCar->p_engineLight) = OFF;
+    }
+    
+    // Release the mutex
+    myCar->p_rawMutex->unlock();
+}
+
+// TASK 3
+void task3_filterSpeed(void *arg)
+{
+    carStructure &myCar = *((carStructure *) arg);
+    
+    // Block access on carStat
+    myCar.p_statMutex->lock();
+    
+    // Copy pointers to values we want to manipulate
+    carStatistics &carStat = *(myCar.p_carStats);
+    vector<float> &speedVec = *(carStat.p_speedVector);
+    
+    // Compute average speed on sizeOfAvg last readings
+    float sum = 0;
+    for (int i = 1; i < carStat.sizeOfAvg + 1; i++)
+    {
+        sum += speedVec[SIZE_SPEED_VECTOR - i];
+    }
+    // Compute the average speed
+    carStat.averageSpeed = sum / carStat.sizeOfAvg;
+    
+    // Release the mutex
+    myCar.p_statMutex->unlock();
+}
+
+// TASK 4
+void task4_showUseOfBrake(void *arg)
+{
+    carStructure &myCar = *((carStructure *) arg);
+    
+    // Block access on RAWValues structure
+    myCar.p_rawMutex->lock();
+    engineRAWValues &p_rawValues = *(myCar.p_rawValues);
+
+    // Read Brake State
+    float brake = p_rawValues.brakeValue;
+    
+    // Release the mutex
+    myCar.p_rawMutex->unlock();
+    
+    // Deal with the copied value
+    monitorValue(brake, *(myCar.p_brakeLight), 0.05);
+}
+
+// TASK 5
+void task5_monitorSpeed(void *arg)
+{
+    carStructure &myCar = *((carStructure *) arg);
+    
+    // Block access on carStat
+    myCar.p_statMutex->lock();
+    // Get an easier access to the vector storing speed values
+    vector<float> &speedVec = *(myCar.p_carStats->p_speedVector);
+    float lastSpeed = speedVec[SIZE_SPEED_VECTOR - 1];
+    
+    // Release the mutex
+    myCar.p_statMutex->unlock();
+    
+    // Deal with the copied value
+    monitorValue(lastSpeed, *(myCar.p_overspeedLight), 141.0/3.6);
+}
+
+// TASK 6
+void task6_writeLCD(void *arg)
+{
+    carStructure &myCar = *((carStructure *) arg);
+    
+    // Block access on carStat
+    myCar.p_statMutex->lock();
+    
+    carStatistics &carStat = *(myCar.p_carStats);
+        
+    // Copy what we want
+    float avgSpeed = carStat.averageSpeed;
+    float distance = carStat.distance;
+    
+    // Release the mutex
+    myCar.p_statMutex->unlock();
+    
+    writeOnLCD(*(myCar.p_lcd), distance, avgSpeed);
+}
+
+// TASK 7
+void task7_sendToMailQueue(void *arg)
+{
+    carStructure *myCar = (carStructure *) arg;
+    
+    // Block access on carStat
+    myCar->p_statMutex->lock();
+    carStatistics &carStat = *(myCar->p_carStats);
+    // Copy what we want
+    float avgSpeed = carStat.averageSpeed;
+    // Release the mutex
+    myCar->p_statMutex->unlock();
+    
+    // Block access on RAWValues structure
+    myCar->p_rawMutex->lock();
+    engineRAWValues &p_rawValues = *(myCar->p_rawValues);
+    // Read Brake State
+    float brake = p_rawValues.brakeValue;
+    float accel = p_rawValues.acceleratorValue;
+    // Release the mutex
+    myCar->p_rawMutex->unlock(); 
+    
+    // New Queue Object
+    mailStruct *mail = myCar->p_mailQueue->alloc();
+    // Copy values
+    mail->accelerator = accel;
+    mail->brake = brake;
+    mail->avgSpeed = avgSpeed;
+    // Add to queue
+    myCar->p_mailQueue->put(mail);
+    myCar->p_PC->printf("\r\nPUT in Queue. In: %i\r\n", myCar->nbElementInQueue);
+    
+    // Lock Counter
+    myCar->p_mailMutex->lock();
+    myCar->nbElementInQueue += 1;
+    // Unlock queue
+    myCar->p_mailMutex->unlock();
+}
+
+// TASK 8
+void task8_dumpMailQueue(void *arg)
+{
+    carStructure *myCar = (carStructure *) arg;
+    
+    // Lock Mail
+    myCar->p_mailMutex->lock();
+    int nbOfElement = myCar->nbElementInQueue;
+    // Unlock Counter
+    myCar->p_mailMutex->unlock();
+    
+    myCar->p_PC->printf("\r\nDumping Queue. In: %i\r\n", nbOfElement);
+    // Go through every element of the queue
+    for (int i = 0; i < nbOfElement ; i++)
+    {
+        // Get Them
+        osEvent evt = myCar->p_mailQueue->get();
+        if (evt.status == osEventMail) 
+        {
+            mailStruct *mail = (mailStruct *)evt.value.p;
+            // Send them to the PC
+            myCar->p_PC->printf("\tElement %i \r\n\t Accel: %.2f\r\n\t",
+                                i+1, mail->accelerator);                  
+            myCar->p_PC->printf(" Brake: %.2f\r\n\t Speed: %.2f\r\n",
+                                mail->brake, (mail->avgSpeed)*3.6);
+            
+            myCar->p_mailQueue->free(mail);
+        }
+    }
+    
+    // Lock Counter
+    myCar->p_mailMutex->lock();
+    // Reset the counter of element in the queue
+    myCar->nbElementInQueue = 0;
+    // Unlock Mail
+    myCar->p_mailMutex->unlock();
+}
+
+// TASK 9
+void task9_readSideLight(void *arg)
+{
+    carStructure *myCar = (carStructure *) arg;
+    
+    if (myCar->p_sideLightSwitch->read())
+    {
+        *(myCar->p_sideLightIndicator) = ON;
+    }
+    else
+    {
+        *(myCar->p_sideLightIndicator) = OFF;
+    }
+}
+
+// TASK 10
+void task10_turnIndic(void *arg)
+{
+    carStructure *myCar = (carStructure *) arg;
+
+    static bool right = false;
+    static bool left  = false;
+    
+    // We check if the pins have changed
+    if ( right != myCar->p_rightSwitch->read() 
+      || left  != myCar->p_leftSwitch->read() )
+    {
+        // If the state of the switch has changed, we update the PWM frequency 
+        // of the LEDs
+        right = myCar->p_rightSwitch->read();
+        left  = myCar->p_leftSwitch->read();
+        updateLEDs(myCar, right, left);
+    }
+}
+