M L / Mbed 2 deprecated ES_Assignment_3_Pub

Embedded Software Assignment 3

Dependencies:   MCP23017 Servo WattBob_TextLCD mbed-rtos mbed

Fork of ES_Assignment_3_Pub by M L

main.cpp@0:6b3496e7a954, 2016-05-27 (annotated)

Committer:
usmb192
Date:
Fri May 27 15:56:36 2016 +0000
Revision:
0:6b3496e7a954
Child:
1:15edb3b6763a
final pub

Who changed what in which revision?

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