Xavier Gouesnard / Mbed 2 deprecated Assignment_3

All the commit done for assignment 3

Dependencies:   MCP23017 Servo WattBob_TextLCD mbed mbed-rtos

main.cpp@3:e8046c9a605c, 2017-04-04 (annotated)

Committer:
xouf2114
Date:
Tue Apr 04 17:45:46 2017 +0000
Revision:
3:e8046c9a605c
Parent:
2:b9c8c2e5fc90
Child:
4:9f5ebb8d85ba
Changes of switches

Who changed what in which revision?

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