Example software of using the mbed-rtos to control a simple vehicle's on board computer
Dependencies: MCP23017 WattBob_TextLCD mbed-rtos mbed
Diff: main.cpp
- Revision:
- 5:4f2a072ed289
- Parent:
- 4:b4bcb329a930
- Child:
- 6:572b9755f2c1
--- 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);