![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Copy_Assignment3
Dependencies: mbed MCP23017 WattBob_TextLCD mbed-rtos
source/my_tasks.cpp
- Committer:
- aoc2
- Date:
- 2018-03-28
- Revision:
- 0:8940db3353d7
File content as of revision 0:8940db3353d7:
#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); } }