All the commit done for assignment 3
Dependencies: MCP23017 Servo WattBob_TextLCD mbed mbed-rtos
main.cpp@6:b6053cb6355e, 2017-04-04 (annotated)
- Committer:
- xouf2114
- Date:
- Tue Apr 04 19:02:02 2017 +0000
- Revision:
- 6:b6053cb6355e
- Parent:
- 5:72f209412e34
- Child:
- 7:535adbafe056
Use the correct frequency
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:9f5ebb8d85ba | 27 | AnalogIn AinBreak(p18); // Port for the Break Value |
xouf2114 | 4:9f5ebb8d85ba | 28 | AnalogIn AinAccel(p19); // 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 | 4:9f5ebb8d85ba | 32 | DigitalIn DinSwitchLindic(p15); // Port for the Left Indicator |
xouf2114 | 5:72f209412e34 | 33 | Servo Odometer(p25); |
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 | 6:b6053cb6355e | 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 | 4:9f5ebb8d85ba | 250 | if(avgSpeed>70.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 | } |