my_tasks.cpp

00001 #include "my_tasks.h"
00002 #include "my_structures.h"
00003 #include "my_tools.h"
00004 
00005 #ifndef __CAR_STATS__
00006 #define __CAT_STATS__
00007 
00008 #define AIR_DENSITY         1.225   // kg/m3
00009 
00010 // TESLA MODEL X
00011 #define DRAG_COEF           0.24
00012 #define REF_AREA            0.62   // m²
00013 
00014 #endif
00015 
00016 using namespace std;
00017 
00018 // TASKS at 10Hz and 5Hz
00019 void timer1(void *arg)
00020 {
00021     static int counterTimer1 = 0;
00022     
00023     counterTimer1++;
00024     task1_readAccelBrake(arg);
00025     
00026     if (counterTimer1 % 2 == 0)
00027     {
00028         task3_filterSpeed(arg);
00029     }
00030 }
00031 
00032 // TASKS at 2Hz, 1 Hz and 0.5 Hz
00033 void timer2(void *arg)
00034 {
00035     static int counterTimer2 = 0;
00036     
00037     counterTimer2++;
00038     task2_readEngineState(arg);
00039     task4_showUseOfBrake(arg);
00040     task6_writeLCD(arg);
00041     
00042     if (counterTimer2 % 2 == 0)
00043     {
00044         task5_monitorSpeed(arg);
00045         task9_readSideLight(arg);
00046     }
00047     if (counterTimer2 % 4 == 0)
00048     {
00049         task10_turnIndic(arg);
00050     }
00051 }
00052 
00053 // TASKS at 0.2 Hz and 0.05 Hz
00054 void timer3(void *arg)
00055 {
00056     static int counterTimer3 = 0;
00057     
00058     counterTimer3++;
00059     task7_sendToMailQueue(arg);
00060     
00061     if (counterTimer3 % 4 == 0)
00062     {
00063         task8_dumpMailQueue(arg);
00064     }
00065 }
00066 
00067 
00068 
00069 
00070 
00071 
00072 
00073 
00074 
00075 // CAR SIMULATOR
00076 void task0_carSim(void *arg)
00077 {
00078     const float dt = 0.05; // 20Hz
00079     carStructure *myCar = (carStructure *) arg;
00080     
00081     // Block access on RAWValues structure
00082     myCar->p_rawMutex->lock();
00083     engineRAWValues &p_rawValues = *(myCar->p_rawValues);
00084     
00085     // Copy the values
00086     int  engine = p_rawValues.engineState;
00087     float accel = p_rawValues.acceleratorValue;
00088     float brake = p_rawValues.brakeValue;
00089     
00090     // Release the mutex once we copied the values
00091     myCar->p_rawMutex->unlock();
00092     
00093 
00094     
00095     // Add it the vector saving the speed values
00096     myCar->p_statMutex->lock();
00097     // Get last speed value stored
00098     float lastSpeed =(*(myCar->p_carStats->p_speedVector))[SIZE_SPEED_VECTOR-1];
00099     myCar->p_statMutex->unlock();
00100 
00101     // ---------------------------------------------------------------------- //
00102     // Acceleration Force
00103     float accelForce = MAX_SPEED * (accel - brake) * engine;
00104     
00105     // ---------------------------------------------------------------------- //
00106     /* 
00107      * DRAG FORCE -> F = 0.5 C ρ A V²
00108      * 
00109      * A = Reference area, m2.
00110      * C = Drag coefficient, unitless.
00111      * F = Drag force, N.
00112      * V = Velocity, m/s.
00113      * ρ = Density of fluid (liquid or gas), kg/m3.
00114      */
00115     float dragForce = 0.5*DRAG_COEF
00116                         *lastSpeed*lastSpeed
00117                         *REF_AREA
00118                         *AIR_DENSITY;
00119     if (lastSpeed == 0) dragForce = 0;
00120 
00121     // ---------------------------------------------------------------------- //
00122     // Compute speed of car on Euler Scheme
00123     float newSpeed = lastSpeed*0.98
00124                     + dt*(accelForce - dragForce);
00125     if (newSpeed < 0) newSpeed = 0;
00126 
00127     // ---------------------------------------------------------------------- //
00128     // Lock Mutex
00129     myCar->p_statMutex->lock();
00130     // Delete first element of vector
00131     deleteFirstValueVec(*(myCar->p_carStats->p_speedVector));
00132     // Add new speed value
00133     myCar->p_carStats->p_speedVector->push_back(newSpeed); // m/s
00134     
00135     // ---------------------------------------------------------------------- //
00136     // Euler Scheme for traveled distance
00137     float distance = myCar->p_carStats->distance + dt*newSpeed; //m
00138     myCar->p_carStats->distance = distance;
00139     // Unlock Mutex
00140     myCar->p_statMutex->unlock();
00141     
00142     // ---------------------------------------------------------------------- //
00143     myCar->p_PC->printf("Accel: %.2f | Brake: %.2f\r\n", accel, brake);
00144     myCar->p_PC->printf("New Speed: %.1f km/h\r\n", newSpeed*3.6);
00145     myCar->p_PC->printf("Traveled Distance: %.3f km\r\n", distance/1000);
00146 }
00147 
00148 // TASK 1
00149 void task1_readAccelBrake(void *arg)
00150 {
00151     carStructure &myCar = *((carStructure *) arg);
00152     
00153     // Block access on RAWValues structure
00154     (myCar.p_rawMutex)->lock();
00155     engineRAWValues &p_rawValues = *(myCar.p_rawValues);
00156     
00157     // Read Accelerator and Brake Values
00158     p_rawValues.acceleratorValue = readAnalogInput(*(myCar.p_accelerator));
00159     p_rawValues.brakeValue       = readAnalogInput(*(myCar.p_brake));
00160     
00161     // Release the mutex
00162     (myCar.p_rawMutex)->unlock();
00163 }
00164 
00165 // TASK 2                  
00166 void task2_readEngineState(void *arg)
00167 {
00168     carStructure *myCar = (carStructure *) arg;
00169     
00170     // Block access on RAWValues structure
00171     myCar->p_rawMutex->lock();
00172     engineRAWValues &p_rawValues = *(myCar->p_rawValues);
00173     
00174     // Read Engine State
00175     int state = myCar->p_engineSwitch->read();
00176     p_rawValues.engineState = state;
00177     if (state)
00178     {
00179         *(myCar->p_engineLight) = ON;
00180     }
00181     else
00182     {
00183         *(myCar->p_engineLight) = OFF;
00184     }
00185     
00186     // Release the mutex
00187     myCar->p_rawMutex->unlock();
00188 }
00189 
00190 // TASK 3
00191 void task3_filterSpeed(void *arg)
00192 {
00193     carStructure &myCar = *((carStructure *) arg);
00194     
00195     // Block access on carStat
00196     myCar.p_statMutex->lock();
00197     
00198     // Copy pointers to values we want to manipulate
00199     carStatistics &carStat = *(myCar.p_carStats);
00200     vector<float> &speedVec = *(carStat.p_speedVector);
00201     
00202     // Compute average speed on sizeOfAvg last readings
00203     float sum = 0;
00204     for (int i = 1; i < carStat.sizeOfAvg + 1; i++)
00205     {
00206         sum += speedVec[SIZE_SPEED_VECTOR - i];
00207     }
00208     // Compute the average speed
00209     carStat.averageSpeed = sum / carStat.sizeOfAvg;
00210     
00211     // Release the mutex
00212     myCar.p_statMutex->unlock();
00213 }
00214 
00215 // TASK 4
00216 void task4_showUseOfBrake(void *arg)
00217 {
00218     carStructure &myCar = *((carStructure *) arg);
00219     
00220     // Block access on RAWValues structure
00221     myCar.p_rawMutex->lock();
00222     engineRAWValues &p_rawValues = *(myCar.p_rawValues);
00223 
00224     // Read Brake State
00225     float brake = p_rawValues.brakeValue;
00226     
00227     // Release the mutex
00228     myCar.p_rawMutex->unlock();
00229     
00230     // Deal with the copied value
00231     monitorValue(brake, *(myCar.p_brakeLight), 0.05);
00232 }
00233 
00234 // TASK 5
00235 void task5_monitorSpeed(void *arg)
00236 {
00237     carStructure &myCar = *((carStructure *) arg);
00238     
00239     // Block access on carStat
00240     myCar.p_statMutex->lock();
00241     // Get an easier access to the vector storing speed values
00242     vector<float> &speedVec = *(myCar.p_carStats->p_speedVector);
00243     float lastSpeed = speedVec[SIZE_SPEED_VECTOR - 1];
00244     
00245     // Release the mutex
00246     myCar.p_statMutex->unlock();
00247     
00248     // Deal with the copied value
00249     monitorValue(lastSpeed, *(myCar.p_overspeedLight), 141.0/3.6);
00250 }
00251 
00252 // TASK 6
00253 void task6_writeLCD(void *arg)
00254 {
00255     carStructure &myCar = *((carStructure *) arg);
00256     
00257     // Block access on carStat
00258     myCar.p_statMutex->lock();
00259     
00260     carStatistics &carStat = *(myCar.p_carStats);
00261         
00262     // Copy what we want
00263     float avgSpeed = carStat.averageSpeed;
00264     float distance = carStat.distance;
00265     
00266     // Release the mutex
00267     myCar.p_statMutex->unlock();
00268     
00269     writeOnLCD(*(myCar.p_lcd), distance, avgSpeed);
00270 }
00271 
00272 // TASK 7
00273 void task7_sendToMailQueue(void *arg)
00274 {
00275     carStructure *myCar = (carStructure *) arg;
00276     
00277     // Block access on carStat
00278     myCar->p_statMutex->lock();
00279     carStatistics &carStat = *(myCar->p_carStats);
00280     // Copy what we want
00281     float avgSpeed = carStat.averageSpeed;
00282     // Release the mutex
00283     myCar->p_statMutex->unlock();
00284     
00285     // Block access on RAWValues structure
00286     myCar->p_rawMutex->lock();
00287     engineRAWValues &p_rawValues = *(myCar->p_rawValues);
00288     // Read Brake State
00289     float brake = p_rawValues.brakeValue;
00290     float accel = p_rawValues.acceleratorValue;
00291     // Release the mutex
00292     myCar->p_rawMutex->unlock(); 
00293     
00294     // New Queue Object
00295     mailStruct *mail = myCar->p_mailQueue->alloc();
00296     // Copy values
00297     mail->accelerator = accel;
00298     mail->brake = brake;
00299     mail->avgSpeed = avgSpeed;
00300     // Add to queue
00301     myCar->p_mailQueue->put(mail);
00302     myCar->p_PC->printf("\r\nPUT in Queue. In: %i\r\n", myCar->nbElementInQueue);
00303     
00304     // Lock Counter
00305     myCar->p_mailMutex->lock();
00306     myCar->nbElementInQueue += 1;
00307     // Unlock queue
00308     myCar->p_mailMutex->unlock();
00309 }
00310 
00311 // TASK 8
00312 void task8_dumpMailQueue(void *arg)
00313 {
00314     carStructure *myCar = (carStructure *) arg;
00315     
00316     // Lock Mail
00317     myCar->p_mailMutex->lock();
00318     int nbOfElement = myCar->nbElementInQueue;
00319     // Unlock Counter
00320     myCar->p_mailMutex->unlock();
00321     
00322     myCar->p_PC->printf("\r\nDumping Queue. In: %i\r\n", nbOfElement);
00323     // Go through every element of the queue
00324     for (int i = 0; i < nbOfElement ; i++)
00325     {
00326         // Get Them
00327         osEvent evt = myCar->p_mailQueue->get();
00328         if (evt.status == osEventMail) 
00329         {
00330             mailStruct *mail = (mailStruct *)evt.value.p;
00331             // Send them to the PC
00332             myCar->p_PC->printf("\tElement %i \r\n\t Accel: %.2f\r\n\t",
00333                                 i+1, mail->accelerator);                  
00334             myCar->p_PC->printf(" Brake: %.2f\r\n\t Speed: %.2f\r\n",
00335                                 mail->brake, (mail->avgSpeed)*3.6);
00336             
00337             myCar->p_mailQueue->free(mail);
00338         }
00339     }
00340     
00341     // Lock Counter
00342     myCar->p_mailMutex->lock();
00343     // Reset the counter of element in the queue
00344     myCar->nbElementInQueue = 0;
00345     // Unlock Mail
00346     myCar->p_mailMutex->unlock();
00347 }
00348 
00349 // TASK 9
00350 void task9_readSideLight(void *arg)
00351 {
00352     carStructure *myCar = (carStructure *) arg;
00353     
00354     if (myCar->p_sideLightSwitch->read())
00355     {
00356         *(myCar->p_sideLightIndicator) = ON;
00357     }
00358     else
00359     {
00360         *(myCar->p_sideLightIndicator) = OFF;
00361     }
00362 }
00363 
00364 // TASK 10
00365 void task10_turnIndic(void *arg)
00366 {
00367     carStructure *myCar = (carStructure *) arg;
00368 
00369     static bool right = false;
00370     static bool left  = false;
00371     
00372     // We check if the pins have changed
00373     if ( right != myCar->p_rightSwitch->read() 
00374       || left  != myCar->p_leftSwitch->read() )
00375     {
00376         // If the state of the switch has changed, we update the PWM frequency 
00377         // of the LEDs
00378         right = myCar->p_rightSwitch->read();
00379         left  = myCar->p_leftSwitch->read();
00380         updateLEDs(myCar, right, left);
00381     }
00382 }
00383