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@1:15edb3b6763a, 2016-05-27 (annotated)

Committer:
usmb192
Date:
Fri May 27 17:02:52 2016 +0000
Revision:
1:15edb3b6763a
Parent:
0:6b3496e7a954 
some cosmetic fixes :)

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