Xavier Gouesnard / Mbed 2 deprecated Assignment_3

All the commit done for assignment 3

Dependencies:   MCP23017 Servo WattBob_TextLCD mbed mbed-rtos

main.cpp@7:535adbafe056, 2017-04-05 (annotated)

Committer:
xouf2114
Date:
Wed Apr 05 11:50:54 2017 +0000
Revision:
7:535adbafe056
Parent:
6:b6053cb6355e
Child:
8:9e8a5014cc0f
Display odometer value

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xouf2114 1:b1b14911f265 1 /*****************************************************************************************************
xouf2114 1:b1b14911f265 2 Assigment 3 Embedded Software
xouf2114 7:535adbafe056 3
xouf2114 1:b1b14911f265 4 This Software is a Car Control Software as no Real Car can be connected it Simulates a car as well.
xouf2114 1:b1b14911f265 5 For the Display it uses an LCD Screen
xouf2114 1:b1b14911f265 6 For the Speedometer it uses a Servo
xouf2114 1:b1b14911f265 7 The Emulation and Synchronisation is implemented using TimerThreads and Semaphores
xouf2114 1:b1b14911f265 8 for the reason of existing thread limits, conventional timers are helping to slicing timeslots
xouf2114 1:b1b14911f265 9
xouf2114 1:b1b14911f265 10 @author Xavier Gouesnard
xouf2114 1:b1b14911f265 11 H00258183
xouf2114 1:b1b14911f265 12 ******************************************************************************************************/
xouf2114 0:8bcf5bf1bbfb 13 #include "mbed.h"
xouf2114 1:b1b14911f265 14 #include "MCP23017.h" // include 16-bit parallel I/O header file
xouf2114 1:b1b14911f265 15 #include "WattBob_TextLCD.h" // include 2*16 character display header file
xouf2114 1:b1b14911f265 16 #include "rtos.h"
xouf2114 1:b1b14911f265 17 #include "Servo.h"
xouf2114 1:b1b14911f265 18 #include <deque>
xouf2114 1:b1b14911f265 19
xouf2114 1:b1b14911f265 20 //#define OS_TIMERCBQS 20 // define a new number of timers we want +1 timer!
xouf2114 1:b1b14911f265 21 //#define OS_TASKCNT 20 // maximum threads
xouf2114 1:b1b14911f265 22
xouf2114 1:b1b14911f265 23 MCP23017 *par_port; // pointer to 16-bit parallel I/O object
xouf2114 1:b1b14911f265 24 WattBob_TextLCD *lcd; // pointer to 2*16 chacater LCD object
xouf2114 1:b1b14911f265 25 Serial serpc(USBTX, USBRX); // serial usb connection tx, rx
xouf2114 4:9f5ebb8d85ba 26 AnalogIn AinBreak(p18); // Port for the Break Value
xouf2114 4:9f5ebb8d85ba 27 AnalogIn AinAccel(p19); // Port for the Accelerator Value
xouf2114 1:b1b14911f265 28 DigitalIn DinSwitchEngine(p11); // Port for the Engine Switch
xouf2114 1:b1b14911f265 29 DigitalIn DinSwitchLight(p12); // Port for the Light Switch
xouf2114 1:b1b14911f265 30 DigitalIn DinSwitchRindic(p13); // Port for the Right Indicator
xouf2114 4:9f5ebb8d85ba 31 DigitalIn DinSwitchLindic(p15); // Port for the Left Indicator
xouf2114 5:72f209412e34 32 Servo Odometer(p25);
xouf2114 1:b1b14911f265 33 DigitalOut LEDSpeedWarning(p8);
xouf2114 1:b1b14911f265 34 DigitalOut DoutLEDLight(LED1); // Output Port for LED1
xouf2114 1:b1b14911f265 35 DigitalOut DoutLEDLeft(LED2); // Output Port for LED2
xouf2114 1:b1b14911f265 36 DigitalOut DoutLEDRight(LED3); // Output Port for LED3
xouf2114 1:b1b14911f265 37 DigitalOut DoutLEDEngine(LED4); // Output Port for LED4
xouf2114 1:b1b14911f265 38 void Timer1_void(void const *args); // Timer 1
xouf2114 1:b1b14911f265 39 void Timer2_void(void const *args); // Timer 2
xouf2114 1:b1b14911f265 40 void Timer3_void(void const *args); // Timer 3
xouf2114 1:b1b14911f265 41
xouf2114 0:8bcf5bf1bbfb 42
xouf2114 1:b1b14911f265 43 // Task Functions
xouf2114 1:b1b14911f265 44 void Task_1_break_accelerate();
xouf2114 1:b1b14911f265 45 void Task_2_read_show_engine_state();
xouf2114 1:b1b14911f265 46 void Task_3_show_odometer();
xouf2114 1:b1b14911f265 47 void Task_4_speed_warning();
xouf2114 1:b1b14911f265 48 void Task_5_update_odometer();
xouf2114 1:b1b14911f265 49 void Task_6_fill_mail_queue();
xouf2114 1:b1b14911f265 50 void Task_7_dump_mail_to_serial();
xouf2114 1:b1b14911f265 51 void Task_8_read_single_side_light();
xouf2114 1:b1b14911f265 52 void Task_9_read_indicators();
xouf2114 1:b1b14911f265 53 void Task_10_calc_avg_speed();
xouf2114 1:b1b14911f265 54 void Task_11_emulate_car();
xouf2114 1:b1b14911f265 55
xouf2114 1:b1b14911f265 56 // Init Variables
xouf2114 1:b1b14911f265 57 int Convert_Hz_to_Ms(double Hz);
xouf2114 1:b1b14911f265 58 float accerlator(0);
xouf2114 1:b1b14911f265 59 float speed(0);
xouf2114 1:b1b14911f265 60 float avgSpeed(0);
xouf2114 1:b1b14911f265 61 float brake(0);
xouf2114 1:b1b14911f265 62 float dist(0);
xouf2114 1:b1b14911f265 63 bool engine(0);
xouf2114 1:b1b14911f265 64 bool indicator_L(1);
xouf2114 1:b1b14911f265 65 bool indicator_R(1);
xouf2114 1:b1b14911f265 66 bool sw_timer1(0);
xouf2114 1:b1b14911f265 67 bool sw_timer11(0);
xouf2114 1:b1b14911f265 68 bool sw_timer2(0);
xouf2114 1:b1b14911f265 69 bool sw_timer21(0);
xouf2114 1:b1b14911f265 70 int sw_timer3(4); // sw_timer3 initalize with a first run
xouf2114 1:b1b14911f265 71
xouf2114 1:b1b14911f265 72 std::deque<float> AvgSpeedDB; // used for storing the average speed
xouf2114 1:b1b14911f265 73 Semaphore SemAvgSpeedDB(1); // declare used Semaphores
xouf2114 1:b1b14911f265 74 Semaphore SemAvgSpeed(1);
xouf2114 1:b1b14911f265 75 Semaphore SemSpeed(1);
xouf2114 1:b1b14911f265 76 Semaphore SemBreak_Accelerate(1);
xouf2114 1:b1b14911f265 77 Semaphore SemDistance(1);
xouf2114 1:b1b14911f265 78 Semaphore SemEngine(1);
xouf2114 1:b1b14911f265 79 Semaphore SemMailCnT(1);
xouf2114 6:b6053cb6355e 80
xouf2114 1:b1b14911f265 81 typedef struct {
xouf2114 1:b1b14911f265 82 float speed;
xouf2114 1:b1b14911f265 83 float accel;
xouf2114 1:b1b14911f265 84 float brake;
xouf2114 1:b1b14911f265 85 } mail_t;
xouf2114 1:b1b14911f265 86 int mailcounter(0); // counts the mails in the queue
xouf2114 1:b1b14911f265 87 Mail<mail_t, 100> mail_box; // the mail queue has a maximum size of 100 mails
xouf2114 0:8bcf5bf1bbfb 88
xouf2114 1:b1b14911f265 89 int main()
xouf2114 1:b1b14911f265 90 {
xouf2114 1:b1b14911f265 91 // 10.0 Hz = 00100 ms
xouf2114 1:b1b14911f265 92 // 2.0 Hz = 00500 ms
xouf2114 1:b1b14911f265 93 // 5.0 Hz = 00200 ms
xouf2114 1:b1b14911f265 94 // 1.0 Hz = 01000 ms
xouf2114 1:b1b14911f265 95 // 0.5 Hz = 02000 ms
xouf2114 1:b1b14911f265 96 // 0.2 Hz = 05000 ms
xouf2114 1:b1b14911f265 97 // 0.05 Hz = 20000 ms
xouf2114 1:b1b14911f265 98
xouf2114 1:b1b14911f265 99
xouf2114 1:b1b14911f265 100
xouf2114 1:b1b14911f265 101
xouf2114 1:b1b14911f265 102 serpc.baud(19200); // setup the bautrate
xouf2114 1:b1b14911f265 103 serpc.printf("Init Software\r\n");
xouf2114 1:b1b14911f265 104 par_port = new MCP23017(p9, p10, 0x40); // initialise 16-bit I/O chip (0x40 = 64)
xouf2114 1:b1b14911f265 105 lcd = new WattBob_TextLCD(par_port); // initialise 2*26 char display
xouf2114 1:b1b14911f265 106 par_port->write_bit(1,BL_BIT); // turn LCD backlight ON
xouf2114 1:b1b14911f265 107 lcd->cls(); // clear display
xouf2114 1:b1b14911f265 108 lcd->locate(0,0); // set cursor to location (0,0) - top left corner
xouf2114 1:b1b14911f265 109 RtosTimer Timer1(Timer1_void,osTimerPeriodic,(void *)NULL); // create the necesarry timers to overcome a thread issue (max threads)
xouf2114 1:b1b14911f265 110 Timer1.start(Convert_Hz_to_Ms(20.0));
xouf2114 1:b1b14911f265 111 RtosTimer Timer2(Timer2_void,osTimerPeriodic,(void *)NULL);
xouf2114 1:b1b14911f265 112 Timer2.start(Convert_Hz_to_Ms(2.0));
xouf2114 1:b1b14911f265 113 RtosTimer Timer3(Timer3_void,osTimerPeriodic,(void *)NULL);
xouf2114 1:b1b14911f265 114 Timer3.start(Convert_Hz_to_Ms(0.2));
xouf2114 1:b1b14911f265 115 Thread::wait(osWaitForever);
xouf2114 1:b1b14911f265 116 }
xouf2114 1:b1b14911f265 117
xouf2114 1:b1b14911f265 118 /*
xouf2114 1:b1b14911f265 119 ##############################################################
xouf2114 1:b1b14911f265 120 Timer 1 runs at 10 Hz
xouf2114 1:b1b14911f265 121 Task_11_emulate_car();
xouf2114 1:b1b14911f265 122 Task_1_break_accelerate();
xouf2114 1:b1b14911f265 123 Task_10_calc_avg_speed();
xouf2114 1:b1b14911f265 124 #############################################################
xouf2114 1:b1b14911f265 125 */
xouf2114 1:b1b14911f265 126
xouf2114 1:b1b14911f265 127 void Timer1_void(void const *args)
xouf2114 1:b1b14911f265 128 {
xouf2114 2:b9c8c2e5fc90 129 Task_11_emulate_car(); // runs every time, so at 10 Hz
xouf2114 1:b1b14911f265 130 sw_timer1 = !sw_timer1;
xouf2114 2:b9c8c2e5fc90 131 if(sw_timer1) { // runs just every second time, so at 10 hz
xouf2114 1:b1b14911f265 132 Task_1_break_accelerate();
xouf2114 1:b1b14911f265 133 sw_timer11 = !sw_timer11;
xouf2114 2:b9c8c2e5fc90 134 if(sw_timer11) { // runs just every fourth time, so at 0.5 hz
xouf2114 1:b1b14911f265 135 Task_10_calc_avg_speed();
xouf2114 1:b1b14911f265 136 }
xouf2114 1:b1b14911f265 137 }
xouf2114 1:b1b14911f265 138 }
xouf2114 1:b1b14911f265 139
xouf2114 1:b1b14911f265 140 /*
xouf2114 1:b1b14911f265 141 ##############################################################
xouf2114 1:b1b14911f265 142 Timer 2 runs at 2 Hz, but starts tasks at 2 Hz, 1 Hz, 0.5 Hz
xouf2114 1:b1b14911f265 143 Task_2_read_show_engine_state();
xouf2114 1:b1b14911f265 144 Task_5_update_odometer();
xouf2114 1:b1b14911f265 145 Updates Indicators
xouf2114 1:b1b14911f265 146 Task_3_show_odometer();
xouf2114 1:b1b14911f265 147 Task_8_read_single_side_light();
xouf2114 1:b1b14911f265 148 Task_4_speed_warning();
xouf2114 1:b1b14911f265 149 Task_9_read_indicators();
xouf2114 1:b1b14911f265 150 #############################################################
xouf2114 1:b1b14911f265 151 */
xouf2114 1:b1b14911f265 152
xouf2114 1:b1b14911f265 153
xouf2114 1:b1b14911f265 154 void Timer2_void(void const *args) // timer runs at 2 hz
xouf2114 1:b1b14911f265 155 {
xouf2114 1:b1b14911f265 156
xouf2114 1:b1b14911f265 157 Task_2_read_show_engine_state();
xouf2114 1:b1b14911f265 158 Task_5_update_odometer();
xouf2114 1:b1b14911f265 159 sw_timer2 = !sw_timer2;
xouf2114 1:b1b14911f265 160
xouf2114 1:b1b14911f265 161 if(indicator_L && indicator_R ) { // Sets the Left and Right Inidcators
xouf2114 1:b1b14911f265 162 DoutLEDLeft=!DoutLEDRight; // needs to get the inverted status of led1 before led1 is changed
xouf2114 1:b1b14911f265 163 DoutLEDRight=!DoutLEDRight;
xouf2114 1:b1b14911f265 164
xouf2114 1:b1b14911f265 165 } else if (!indicator_R && !indicator_L) {
xouf2114 1:b1b14911f265 166 DoutLEDLeft=0;
xouf2114 1:b1b14911f265 167 DoutLEDRight=0;
xouf2114 1:b1b14911f265 168 }
xouf2114 1:b1b14911f265 169
xouf2114 1:b1b14911f265 170 if(sw_timer2)
xouf2114 1:b1b14911f265 171 { // runs just every second time, so at 1 hz
xouf2114 1:b1b14911f265 172 Task_3_show_odometer();
xouf2114 1:b1b14911f265 173 Task_8_read_single_side_light();
xouf2114 1:b1b14911f265 174 sw_timer21 = !sw_timer21;
xouf2114 1:b1b14911f265 175
xouf2114 1:b1b14911f265 176 if (!indicator_R && indicator_L)
xouf2114 1:b1b14911f265 177 { // switch the left / right indicator
xouf2114 1:b1b14911f265 178 DoutLEDRight=0;
xouf2114 1:b1b14911f265 179 DoutLEDLeft=!DoutLEDLeft;
xouf2114 1:b1b14911f265 180 } else if(indicator_R && !indicator_L) {
xouf2114 1:b1b14911f265 181 DoutLEDRight=!DoutLEDRight;
xouf2114 1:b1b14911f265 182 DoutLEDLeft=0;
xouf2114 1:b1b14911f265 183 }
xouf2114 1:b1b14911f265 184 if(sw_timer21) { // runs just every second time, so at 0.5 hz
xouf2114 1:b1b14911f265 185 Task_4_speed_warning();
xouf2114 1:b1b14911f265 186 Task_9_read_indicators();
xouf2114 1:b1b14911f265 187 }
xouf2114 0:8bcf5bf1bbfb 188 }
xouf2114 0:8bcf5bf1bbfb 189 }
xouf2114 1:b1b14911f265 190 /*
xouf2114 1:b1b14911f265 191 ##############################################################
xouf2114 1:b1b14911f265 192 Timer 3 runs at 0.2 Hz, but starts tasks at 0.2 Hz and 0.05 Hz
xouf2114 1:b1b14911f265 193 Task_6_fill_mail_queue();
xouf2114 1:b1b14911f265 194 Task_7_dump_mail_to_serial();
xouf2114 1:b1b14911f265 195 ##############################################################
xouf2114 1:b1b14911f265 196 */
xouf2114 1:b1b14911f265 197 void Timer3_void(void const *args) // timer runs at 0.2 hz
xouf2114 1:b1b14911f265 198 {
xouf2114 1:b1b14911f265 199 Task_6_fill_mail_queue();
xouf2114 1:b1b14911f265 200 if((sw_timer3%4)==0) { // task runs at 0.05 Hz
xouf2114 1:b1b14911f265 201 Task_7_dump_mail_to_serial(); // dump the queue to serial
xouf2114 1:b1b14911f265 202 sw_timer3=0; // reset the timer
xouf2114 1:b1b14911f265 203 }
xouf2114 1:b1b14911f265 204 sw_timer3++;
xouf2114 1:b1b14911f265 205 }
xouf2114 1:b1b14911f265 206 /*
xouf2114 1:b1b14911f265 207 Reads the brake / acceleration of the car
xouf2114 1:b1b14911f265 208 */
xouf2114 1:b1b14911f265 209 void Task_1_break_accelerate()
xouf2114 1:b1b14911f265 210 {
xouf2114 1:b1b14911f265 211 // Let the Semaphores wait
xouf2114 1:b1b14911f265 212 SemBreak_Accelerate.wait();
xouf2114 1:b1b14911f265 213 accerlator = AinAccel; // save the accerlator value
xouf2114 1:b1b14911f265 214 brake = AinBreak; // save the brake value
xouf2114 2:b9c8c2e5fc90 215
xouf2114 1:b1b14911f265 216 // Let the Semaphores release
xouf2114 1:b1b14911f265 217 SemBreak_Accelerate.release();
xouf2114 1:b1b14911f265 218 }
xouf2114 1:b1b14911f265 219 /*
xouf2114 1:b1b14911f265 220 Reads the Engine On/Off Switch and displays its state
xouf2114 1:b1b14911f265 221 */
xouf2114 1:b1b14911f265 222 void Task_2_read_show_engine_state()
xouf2114 1:b1b14911f265 223 {
xouf2114 1:b1b14911f265 224 // Let the Semaphores wait
xouf2114 1:b1b14911f265 225 SemEngine.wait();
xouf2114 1:b1b14911f265 226 engine = DinSwitchEngine; // read the engine state
xouf2114 1:b1b14911f265 227 DoutLEDEngine = engine; // write the engine state
xouf2114 1:b1b14911f265 228 // Let the Semaphores release
xouf2114 1:b1b14911f265 229 SemEngine.release();
xouf2114 1:b1b14911f265 230 }
xouf2114 1:b1b14911f265 231 /*
xouf2114 1:b1b14911f265 232 Updates the Odometer (Servo Motor)
xouf2114 1:b1b14911f265 233 */
xouf2114 1:b1b14911f265 234 void Task_3_show_odometer()
xouf2114 1:b1b14911f265 235 {
xouf2114 1:b1b14911f265 236 // Let the Semaphores wait
xouf2114 1:b1b14911f265 237 SemAvgSpeed.wait();
xouf2114 1:b1b14911f265 238 Odometer = avgSpeed/250.0; // Calculate the odometer
xouf2114 1:b1b14911f265 239 // Let the Semaphores release
xouf2114 1:b1b14911f265 240 SemAvgSpeed.release();
xouf2114 1:b1b14911f265 241 }
xouf2114 1:b1b14911f265 242 /*
xouf2114 1:b1b14911f265 243 Indicates a Speed warning at 75 Mph
xouf2114 1:b1b14911f265 244 */
xouf2114 1:b1b14911f265 245 void Task_4_speed_warning()
xouf2114 1:b1b14911f265 246 {
xouf2114 1:b1b14911f265 247 // Let the Semaphores wait
xouf2114 1:b1b14911f265 248 SemAvgSpeed.wait();
xouf2114 4:9f5ebb8d85ba 249 if(avgSpeed>70.0) // check our speed
xouf2114 1:b1b14911f265 250 LEDSpeedWarning = !LEDSpeedWarning; // and switch the Warning on/off
xouf2114 1:b1b14911f265 251 else
xouf2114 1:b1b14911f265 252 LEDSpeedWarning = 0;
xouf2114 1:b1b14911f265 253 // Let the Semaphores release
xouf2114 1:b1b14911f265 254 SemAvgSpeed.release();
xouf2114 1:b1b14911f265 255 }
xouf2114 1:b1b14911f265 256 /*
xouf2114 1:b1b14911f265 257 Updates the LCD Display
xouf2114 1:b1b14911f265 258 */
xouf2114 1:b1b14911f265 259 void Task_5_update_odometer()
xouf2114 1:b1b14911f265 260 {
xouf2114 1:b1b14911f265 261 // Let the Semaphores wait
xouf2114 1:b1b14911f265 262 SemDistance.wait();
xouf2114 1:b1b14911f265 263 SemAvgSpeed.wait();
xouf2114 1:b1b14911f265 264 lcd->locate(0,0); // set cursor to location (0,0) - top left corner
xouf2114 1:b1b14911f265 265 lcd->printf("s: %5.0f",avgSpeed);
xouf2114 1:b1b14911f265 266 lcd->locate(1,0);
xouf2114 1:b1b14911f265 267 lcd->printf("d: %5.0f",dist);
xouf2114 1:b1b14911f265 268 // Let the Semaphores release
xouf2114 1:b1b14911f265 269 SemDistance.release();
xouf2114 1:b1b14911f265 270 SemAvgSpeed.release();
xouf2114 1:b1b14911f265 271 }
xouf2114 1:b1b14911f265 272 /*
xouf2114 1:b1b14911f265 273 Reads the Left and Right Inidcator
xouf2114 1:b1b14911f265 274 */
xouf2114 1:b1b14911f265 275 void Task_6_fill_mail_queue()
xouf2114 1:b1b14911f265 276 {
xouf2114 1:b1b14911f265 277 // Let the Semaphores wait
xouf2114 1:b1b14911f265 278 SemMailCnT.wait();
xouf2114 1:b1b14911f265 279 SemBreak_Accelerate.wait();
xouf2114 1:b1b14911f265 280 SemSpeed.wait();
xouf2114 1:b1b14911f265 281 mail_t *mail = mail_box.alloc(); // reserve the space for our new message
xouf2114 1:b1b14911f265 282 mail->speed = speed; // fill with values
xouf2114 1:b1b14911f265 283 mail->accel = accerlator;
xouf2114 1:b1b14911f265 284 mail->brake = brake;
xouf2114 1:b1b14911f265 285 mail_box.put(mail); // put the new message into the mail queue
xouf2114 1:b1b14911f265 286 mailcounter++;
xouf2114 1:b1b14911f265 287 // Let the Semaphores release
xouf2114 1:b1b14911f265 288 SemBreak_Accelerate.release();
xouf2114 1:b1b14911f265 289 SemSpeed.release();
xouf2114 1:b1b14911f265 290 SemMailCnT.release();
xouf2114 1:b1b14911f265 291 }
xouf2114 1:b1b14911f265 292 /*
xouf2114 1:b1b14911f265 293 Reads the Mail Queue and Sends the Content to the Serial Port
xouf2114 1:b1b14911f265 294 */
xouf2114 1:b1b14911f265 295 void Task_7_dump_mail_to_serial()
xouf2114 1:b1b14911f265 296 {
xouf2114 1:b1b14911f265 297 // Let the Semaphores wait
xouf2114 1:b1b14911f265 298 SemMailCnT.wait();
xouf2114 1:b1b14911f265 299 while(mailcounter) { // as long as we got mail
xouf2114 1:b1b14911f265 300 osEvent evt = mail_box.get(); // we are getting them
xouf2114 1:b1b14911f265 301 if (evt.status == osEventMail) {
xouf2114 1:b1b14911f265 302 mail_t *mail = (mail_t*)evt.value.p; // print the mail to serial
xouf2114 1:b1b14911f265 303 serpc.printf("\nspeed: %.0f \n\r" , mail->speed);
xouf2114 1:b1b14911f265 304 serpc.printf("accerlator: %.2f\n\r" , mail->accel);
xouf2114 1:b1b14911f265 305 serpc.printf("brake: %.2f\n\r", mail->brake);
xouf2114 1:b1b14911f265 306 mail_box.free(mail); // clear up the mailbox
xouf2114 1:b1b14911f265 307 }
xouf2114 1:b1b14911f265 308 mailcounter--;
xouf2114 1:b1b14911f265 309 }
xouf2114 1:b1b14911f265 310 // Release the Semaphores
xouf2114 1:b1b14911f265 311 SemMailCnT.release();
xouf2114 1:b1b14911f265 312 }
xouf2114 1:b1b14911f265 313 /*
xouf2114 1:b1b14911f265 314 Single Side Light
xouf2114 1:b1b14911f265 315 */
xouf2114 1:b1b14911f265 316 void Task_8_read_single_side_light()
xouf2114 1:b1b14911f265 317 {
xouf2114 1:b1b14911f265 318 DoutLEDLight = DinSwitchLight; // Reading the value
xouf2114 1:b1b14911f265 319 }
xouf2114 1:b1b14911f265 320 /*
xouf2114 1:b1b14911f265 321 Reads the Left and Right Inidcator
xouf2114 1:b1b14911f265 322 */
xouf2114 1:b1b14911f265 323 void Task_9_read_indicators()
xouf2114 1:b1b14911f265 324 {
xouf2114 1:b1b14911f265 325 indicator_R = DinSwitchRindic; // Reading the value
xouf2114 1:b1b14911f265 326 indicator_L = DinSwitchLindic; // Reading the value
xouf2114 1:b1b14911f265 327 }
xouf2114 1:b1b14911f265 328 /*
xouf2114 1:b1b14911f265 329 Calculates the Average Speed
xouf2114 1:b1b14911f265 330 */
xouf2114 1:b1b14911f265 331 void Task_10_calc_avg_speed()
xouf2114 1:b1b14911f265 332 {
xouf2114 1:b1b14911f265 333 // Let the Semaphores wait
xouf2114 1:b1b14911f265 334 SemAvgSpeed.wait();
xouf2114 1:b1b14911f265 335 SemAvgSpeedDB.wait();
xouf2114 1:b1b14911f265 336 float sum(0);
xouf2114 1:b1b14911f265 337 for(deque<float>::const_iterator i = AvgSpeedDB.begin(); i != AvgSpeedDB.end(); ++i)
xouf2114 1:b1b14911f265 338 sum+= *i; // calculate the average by iterating over the queue
xouf2114 1:b1b14911f265 339 avgSpeed = sum/AvgSpeedDB.size();
xouf2114 1:b1b14911f265 340 // Release the Semaphores
xouf2114 1:b1b14911f265 341 SemAvgSpeedDB.release();
xouf2114 1:b1b14911f265 342 SemAvgSpeed.release();
xouf2114 1:b1b14911f265 343 }
xouf2114 1:b1b14911f265 344 /*
xouf2114 1:b1b14911f265 345 Emulates the car
xouf2114 1:b1b14911f265 346 */
xouf2114 1:b1b14911f265 347 void Task_11_emulate_car()
xouf2114 1:b1b14911f265 348 {
xouf2114 1:b1b14911f265 349 // Let the Semaphores wait
xouf2114 1:b1b14911f265 350 SemAvgSpeed.wait();
xouf2114 1:b1b14911f265 351 SemAvgSpeedDB.wait();
xouf2114 1:b1b14911f265 352 SemDistance.wait();
xouf2114 1:b1b14911f265 353 SemBreak_Accelerate.wait();
xouf2114 1:b1b14911f265 354 SemSpeed.wait();
xouf2114 1:b1b14911f265 355 SemEngine.wait();
xouf2114 1:b1b14911f265 356 if(accerlator<=brake || !engine) // are we braking more than accelerating? is the engine on?
xouf2114 1:b1b14911f265 357 speed = 0;
xouf2114 1:b1b14911f265 358 else
xouf2114 1:b1b14911f265 359 speed = (accerlator-brake) *0.5 +speed;
xouf2114 1:b1b14911f265 360 if(speed>250)
xouf2114 1:b1b14911f265 361 speed=250; // maximum speed
xouf2114 1:b1b14911f265 362 if(AvgSpeedDB.size()>=4) // if we already got 4 values, we have to
xouf2114 1:b1b14911f265 363 AvgSpeedDB.pop_front(); // make space by deleting the oldest value
xouf2114 1:b1b14911f265 364 AvgSpeedDB.push_back(speed); // safe a new reading
xouf2114 1:b1b14911f265 365 dist += speed * 1.0/20.0; // runs at 20 Hz so we have to take this into account
xouf2114 1:b1b14911f265 366 // Release the Semaphores
xouf2114 1:b1b14911f265 367 SemDistance.release();
xouf2114 1:b1b14911f265 368 SemAvgSpeed.release();
xouf2114 1:b1b14911f265 369 SemAvgSpeedDB.release();
xouf2114 1:b1b14911f265 370 SemBreak_Accelerate.release();
xouf2114 1:b1b14911f265 371 SemSpeed.release();
xouf2114 1:b1b14911f265 372 SemEngine.release();
xouf2114 1:b1b14911f265 373 }
xouf2114 1:b1b14911f265 374 /*
xouf2114 1:b1b14911f265 375 Function used for converting Hz to Ms for a Steps
xouf2114 1:b1b14911f265 376 */
xouf2114 1:b1b14911f265 377 int Convert_Hz_to_Ms(double Hz)
xouf2114 1:b1b14911f265 378 {
xouf2114 1:b1b14911f265 379 return 1000.0 / Hz;
xouf2114 1:b1b14911f265 380 }