Copy_Assignment3
Dependencies: mbed MCP23017 WattBob_TextLCD mbed-rtos
Diff: source/my_tasks.cpp
- Revision:
- 0:8940db3353d7
diff -r 000000000000 -r 8940db3353d7 source/my_tasks.cpp --- /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); + } +} +