Muaiyd Al-Zandi / Mbed 2 deprecated CAR_Simulation

This is car control simulation by using Mbed controller and real time operating system.

Dependencies:   MCP23017 Servo WattBob_TextLCD mbed-rtos mbed

Fork of Ass3 by Muaiyd Al-Zandi

CAR.cpp@11:7f2414ecb7ee, 2014-05-06 (annotated)

Committer:
muaiyd
Date:
Tue May 06 09:11:01 2014 +0000
Revision:
11:7f2414ecb7ee
Parent:
10:2522e3878e1c
Child:
12:8eb2c1cccee6
Last change; Optemization

Who changed what in which revision?

UserRevisionLine numberNew contents of line
muaiyd 0:68ce46607848 1 #include "CAR.h"
muaiyd 11:7f2414ecb7ee 2
muaiyd 11:7f2414ecb7ee 3 //Define semaphores to manage accessing the viriables
muaiyd 1:b409ad65466a 4 Semaphore CAR_MAIL_SEM(1);
muaiyd 9:d86a6b8cdfa4 5 Semaphore INPUT_SEM(3);
muaiyd 9:d86a6b8cdfa4 6 Semaphore SWITCH_SEM(2);
muaiyd 11:7f2414ecb7ee 7 Semaphore AVR_SPEED_SEM(2);
muaiyd 11:7f2414ecb7ee 8 /* Two counter the first is counting the number of element that
muaiyd 11:7f2414ecb7ee 9 was sended to the MAIL. The another is to count the number of
muaiyd 11:7f2414ecb7ee 10 element that dumped to a file */
muaiyd 1:b409ad65466a 11 uint32_t Element_Counter_W = 0;
muaiyd 1:b409ad65466a 12 uint32_t Element_Counter_R = 0;
muaiyd 11:7f2414ecb7ee 13 // Initialise a mail with 100 element
muaiyd 11:7f2414ecb7ee 14 static Mail<CAR_MAIL, 100> mail_box;
muaiyd 9:d86a6b8cdfa4 15
muaiyd 7:a92da438d06c 16 bool LSide_Indicator_Value;
muaiyd 7:a92da438d06c 17 bool RSide_Indicator_Value;
muaiyd 9:d86a6b8cdfa4 18 // Create the local filesystem under the name "local"
muaiyd 9:d86a6b8cdfa4 19 LocalFileSystem local("local");
muaiyd 9:d86a6b8cdfa4 20
muaiyd 1:b409ad65466a 21 CAR::CAR(){
muaiyd 1:b409ad65466a 22 Port.write_bit(1,BL_BIT);
muaiyd 11:7f2414ecb7ee 23 // Open "out.txt" on the local file system for writing
muaiyd 11:7f2414ecb7ee 24 FILE *fp = fopen("/local/Car_Values.csv", "w");
muaiyd 9:d86a6b8cdfa4 25 fprintf(fp, "Counter,Average_Speed,Accelerometer_Value,Brake_Value\r\n");
muaiyd 9:d86a6b8cdfa4 26 fclose(fp);
muaiyd 1:b409ad65466a 27 }
muaiyd 11:7f2414ecb7ee 28 // This two functions are save and read the ODO value in a register
muaiyd 11:7f2414ecb7ee 29 // in order to safe it when the mbed restart
muaiyd 1:b409ad65466a 30 void CAR::SAVE_ODO(float value) {
muaiyd 1:b409ad65466a 31 LPC_RTC->GPREG0 = *((uint32_t*)&value);
muaiyd 1:b409ad65466a 32 }
muaiyd 1:b409ad65466a 33 float CAR::GET_ODO() {
muaiyd 1:b409ad65466a 34 return *((float*)&(LPC_RTC->GPREG0));
muaiyd 1:b409ad65466a 35 }
muaiyd 11:7f2414ecb7ee 36
muaiyd 11:7f2414ecb7ee 37 /* Read the accelerometer and brake value every 100 mSec and calculate the speed.
muaiyd 11:7f2414ecb7ee 38 Also save them in variabls.The speed is saved in an array of three element
muaiyd 11:7f2414ecb7ee 39 so the past three values of speed is saved. */
muaiyd 1:b409ad65466a 40 void CAR::Accelero_Brake_Read(void const *args){
muaiyd 0:68ce46607848 41 while (true) {
muaiyd 7:a92da438d06c 42 INPUT_SEM.wait();
muaiyd 1:b409ad65466a 43 Accelerometer_Value = Accelerometer.read();
muaiyd 1:b409ad65466a 44 Brake_Value = Brake.read();
muaiyd 1:b409ad65466a 45 EngineStat = EngineSwitch;
muaiyd 1:b409ad65466a 46 Speed[Counter] = Accelerometer_Value * MaxSpeed * (1 - Brake_Value) * EngineStat ;
muaiyd 1:b409ad65466a 47 Counter++;
muaiyd 1:b409ad65466a 48 if(Counter > 2) Counter = 0;
muaiyd 7:a92da438d06c 49 INPUT_SEM.release();
muaiyd 1:b409ad65466a 50 Thread::wait(100);
muaiyd 1:b409ad65466a 51 }
muaiyd 1:b409ad65466a 52 }
muaiyd 1:b409ad65466a 53
muaiyd 11:7f2414ecb7ee 54 // Average filter the speed every 200 mSec
muaiyd 1:b409ad65466a 55 void CAR::Average_Speed_Measure(void const *args) {
muaiyd 1:b409ad65466a 56 while (true) {
muaiyd 7:a92da438d06c 57 INPUT_SEM.wait();
muaiyd 11:7f2414ecb7ee 58 AVR_SPEED_SEM.wait();
muaiyd 1:b409ad65466a 59 Average_Speed = ( Speed[0] + Speed[1] + Speed[2] )/3 ;
muaiyd 7:a92da438d06c 60 INPUT_SEM.release();
muaiyd 11:7f2414ecb7ee 61 AVR_SPEED_SEM.release();
muaiyd 1:b409ad65466a 62 Thread::wait(200);
muaiyd 1:b409ad65466a 63 }
muaiyd 1:b409ad65466a 64 }
muaiyd 1:b409ad65466a 65
muaiyd 11:7f2414ecb7ee 66 // Update the servo every 1 Sec with the average speed value
muaiyd 1:b409ad65466a 67 void CAR::Average_Speed_Show(void const *args){
muaiyd 1:b409ad65466a 68 while(1){
muaiyd 11:7f2414ecb7ee 69 AVR_SPEED_SEM.wait();
muaiyd 7:a92da438d06c 70 SpeedShow_Servo = 1.0 - (float)Average_Speed / (float)MaxSpeed ;
muaiyd 11:7f2414ecb7ee 71 AVR_SPEED_SEM.release();
muaiyd 0:68ce46607848 72 Thread::wait(1000);
muaiyd 0:68ce46607848 73 }
muaiyd 1:b409ad65466a 74 }
muaiyd 11:7f2414ecb7ee 75
muaiyd 11:7f2414ecb7ee 76 // Check the average speed if it over 70 MPH every 2Sec
muaiyd 1:b409ad65466a 77 void CAR::OverSpeed(void const *args){
muaiyd 1:b409ad65466a 78 while(true){
muaiyd 11:7f2414ecb7ee 79 AVR_SPEED_SEM.wait();
muaiyd 1:b409ad65466a 80 if(Average_Speed > 70)
muaiyd 1:b409ad65466a 81 IsOverSpeed = 1;
muaiyd 1:b409ad65466a 82
muaiyd 1:b409ad65466a 83 else
muaiyd 1:b409ad65466a 84 IsOverSpeed = 0;
muaiyd 11:7f2414ecb7ee 85 AVR_SPEED_SEM.release();
muaiyd 7:a92da438d06c 86 Thread::wait(2000);
muaiyd 1:b409ad65466a 87 }
muaiyd 1:b409ad65466a 88 }
muaiyd 11:7f2414ecb7ee 89 /* To task in this function becuase boath are repeted every 0.5 Sec
muaiyd 11:7f2414ecb7ee 90 1- Update the ODOmeter
muaiyd 11:7f2414ecb7ee 91 2- Read the indecator swetches */
muaiyd 7:a92da438d06c 92 void CAR::Odo_Show_Indicator_Switch_Read(void const *args){
muaiyd 1:b409ad65466a 93 while(true){
muaiyd 1:b409ad65466a 94 LCD.locate(0,0);
muaiyd 11:7f2414ecb7ee 95 AVR_SPEED_SEM.wait();
muaiyd 7:a92da438d06c 96 Odometer_Value = GET_ODO() + Average_Speed / 7200.0 ;
muaiyd 7:a92da438d06c 97 SAVE_ODO(Odometer_Value);
muaiyd 11:7f2414ecb7ee 98 LCD.printf("Speed %3D MPH",Average_Speed);
muaiyd 1:b409ad65466a 99 LCD.locate(1,0);
muaiyd 11:7f2414ecb7ee 100 LCD.printf("ODO %4.2f M",GET_ODO());
muaiyd 11:7f2414ecb7ee 101 AVR_SPEED_SEM.release();
muaiyd 11:7f2414ecb7ee 102
muaiyd 7:a92da438d06c 103 SWITCH_SEM.wait();
muaiyd 7:a92da438d06c 104 LSide_Indicator_Value = LSide_Indicator_Switch;
muaiyd 7:a92da438d06c 105 RSide_Indicator_Value = RSide_Indicator_Switch;
muaiyd 7:a92da438d06c 106 SWITCH_SEM.release();
muaiyd 11:7f2414ecb7ee 107
muaiyd 1:b409ad65466a 108 Thread::wait(500);
muaiyd 1:b409ad65466a 109 }
muaiyd 1:b409ad65466a 110 }
muaiyd 11:7f2414ecb7ee 111
muaiyd 11:7f2414ecb7ee 112 /* Send the values of the Brake, Accelerometer, and Speed to
muaiyd 11:7f2414ecb7ee 113 an mail queue every 5 Sec. */
muaiyd 9:d86a6b8cdfa4 114 void CAR::SEND_CAR_VALUES (void const *args) {
muaiyd 1:b409ad65466a 115 while (true) {
muaiyd 1:b409ad65466a 116 CAR_MAIL *mail = mail_box.alloc();
muaiyd 1:b409ad65466a 117 CAR_MAIL_SEM.wait();
muaiyd 11:7f2414ecb7ee 118
muaiyd 11:7f2414ecb7ee 119 AVR_SPEED_SEM.wait();
muaiyd 11:7f2414ecb7ee 120 mail->Mail_Average_Speed = Average_Speed;
muaiyd 11:7f2414ecb7ee 121 AVR_SPEED_SEM.release();
muaiyd 11:7f2414ecb7ee 122
muaiyd 9:d86a6b8cdfa4 123 INPUT_SEM.wait();
muaiyd 1:b409ad65466a 124 mail->Mail_Accelerometer_Value = Accelerometer_Value;
muaiyd 1:b409ad65466a 125 mail->Mail_Brake_Value = Brake_Value;
muaiyd 1:b409ad65466a 126 mail->Counter = Element_Counter_W ;
muaiyd 1:b409ad65466a 127 mail_box.put(mail);
muaiyd 1:b409ad65466a 128 Element_Counter_W++;
muaiyd 7:a92da438d06c 129 CAR_MAIL_SEM.release();
muaiyd 9:d86a6b8cdfa4 130 INPUT_SEM.release();
muaiyd 11:7f2414ecb7ee 131
muaiyd 7:a92da438d06c 132 Thread::wait(5000);
muaiyd 1:b409ad65466a 133 }
muaiyd 1:b409ad65466a 134 }
muaiyd 11:7f2414ecb7ee 135
muaiyd 11:7f2414ecb7ee 136 //Saving the memory pool content on the serial port
muaiyd 1:b409ad65466a 137 void CAR::DUMP_CAR_VALUES_En (void const *args) {
muaiyd 1:b409ad65466a 138 while (true) {
muaiyd 1:b409ad65466a 139 CAR_MAIL_SEM.wait();
muaiyd 1:b409ad65466a 140 while (Element_Counter_W > Element_Counter_R)
muaiyd 11:7f2414ecb7ee 141 DUMP_CAR_VALUES();
muaiyd 11:7f2414ecb7ee 142
muaiyd 1:b409ad65466a 143 CAR_MAIL_SEM.release();
muaiyd 6:5037779f6a55 144 Thread::wait(20000);
muaiyd 1:b409ad65466a 145 }
muaiyd 1:b409ad65466a 146 }
muaiyd 1:b409ad65466a 147 void CAR::DUMP_CAR_VALUES () {
muaiyd 1:b409ad65466a 148 osEvent evt = mail_box.get(10);
muaiyd 1:b409ad65466a 149 CAR_MAIL *mail = (CAR_MAIL*)evt.value.p;
muaiyd 9:d86a6b8cdfa4 150 if (evt.status == osEventMail) {
muaiyd 9:d86a6b8cdfa4 151 FILE *fp = fopen("/local/Car_Values.csv", "a"); // Open "out.txt" on the local file system for writing
muaiyd 9:d86a6b8cdfa4 152 fprintf(fp,"%i ," , mail->Counter);
muaiyd 9:d86a6b8cdfa4 153 fprintf(fp,"%i ,", mail->Mail_Average_Speed);
muaiyd 9:d86a6b8cdfa4 154 fprintf(fp,"%.3f ," , mail->Mail_Accelerometer_Value);
muaiyd 9:d86a6b8cdfa4 155 fprintf(fp,"%.3f ", mail->Mail_Brake_Value);
muaiyd 9:d86a6b8cdfa4 156 fprintf(fp,"\r\n");
muaiyd 9:d86a6b8cdfa4 157 fclose(fp);
muaiyd 1:b409ad65466a 158 mail_box.free(mail);
muaiyd 1:b409ad65466a 159 Element_Counter_R++;
muaiyd 1:b409ad65466a 160 }
muaiyd 7:a92da438d06c 161 }
muaiyd 11:7f2414ecb7ee 162
muaiyd 11:7f2414ecb7ee 163 // Flashing the indecator light
muaiyd 7:a92da438d06c 164 void CAR::Side_Light_Flash(void const *args){
muaiyd 7:a92da438d06c 165 while(true){
muaiyd 7:a92da438d06c 166 SWITCH_SEM.wait();
muaiyd 11:7f2414ecb7ee 167 // If both side are enabled
muaiyd 8:6e55db96c11c 168 if((LSide_Indicator_Value == 1) && (RSide_Indicator_Value == 1) ){
muaiyd 8:6e55db96c11c 169 L_Side_Indicator = L_Side_Indicator ^ 1;
muaiyd 8:6e55db96c11c 170 R_Side_Indicator = L_Side_Indicator;
muaiyd 8:6e55db96c11c 171 }
muaiyd 11:7f2414ecb7ee 172 //If just one side enabled
muaiyd 8:6e55db96c11c 173 else if(RSide_Indicator_Value == 1){
muaiyd 8:6e55db96c11c 174 R_Side_Indicator = R_Side_Indicator ^ 1;
muaiyd 8:6e55db96c11c 175 L_Side_Indicator = 0;
muaiyd 8:6e55db96c11c 176 }
muaiyd 8:6e55db96c11c 177 else if(LSide_Indicator_Value == 1){
muaiyd 8:6e55db96c11c 178 L_Side_Indicator = L_Side_Indicator ^ 1;
muaiyd 8:6e55db96c11c 179 R_Side_Indicator = 0;
muaiyd 8:6e55db96c11c 180 }
muaiyd 11:7f2414ecb7ee 181 //Neither are enabled
muaiyd 8:6e55db96c11c 182 else{
muaiyd 8:6e55db96c11c 183 L_Side_Indicator = 0;
muaiyd 8:6e55db96c11c 184 R_Side_Indicator = 0 ;
muaiyd 8:6e55db96c11c 185 }
muaiyd 8:6e55db96c11c 186 SWITCH_SEM.release();
muaiyd 8:6e55db96c11c 187 Thread::wait(1000);
muaiyd 8:6e55db96c11c 188 }
muaiyd 8:6e55db96c11c 189 }
muaiyd 11:7f2414ecb7ee 190 // Check the side light swetches and turn on or off the light
muaiyd 8:6e55db96c11c 191 void CAR::Side_Light(void const *args){
muaiyd 8:6e55db96c11c 192 while(true){
muaiyd 8:6e55db96c11c 193 if(LSide_Light_Switch){
muaiyd 8:6e55db96c11c 194 L_Side_Light = 1;
muaiyd 7:a92da438d06c 195 }
muaiyd 7:a92da438d06c 196 else{
muaiyd 7:a92da438d06c 197 L_Side_Light = 0;
muaiyd 7:a92da438d06c 198 }
muaiyd 8:6e55db96c11c 199 if(RSide_Light_Switch){
muaiyd 8:6e55db96c11c 200 R_Side_Light = 1;
muaiyd 7:a92da438d06c 201 }
muaiyd 7:a92da438d06c 202 else{
muaiyd 8:6e55db96c11c 203 R_Side_Light = 0;
muaiyd 7:a92da438d06c 204 }
muaiyd 7:a92da438d06c 205 Thread::wait(1000);
muaiyd 7:a92da438d06c 206 }
muaiyd 0:68ce46607848 207 }