Xavier Gouesnard / Mbed 2 deprecated Assignment_3

All the commit done for assignment 3

Dependencies:   MCP23017 Servo WattBob_TextLCD mbed mbed-rtos

main.cpp@9:8ac963c7e3a1, 2017-04-05 (annotated)

Committer:
xouf2114
Date:
Wed Apr 05 13:08:32 2017 +0000
Revision:
9:8ac963c7e3a1
Parent:
8:9e8a5014cc0f 
Commit

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