All tasks complete
Dependencies: mbed MCP23017 mbed-rtos WattBob_TextLCD
assignment3tasks.cpp@14:25241ed5b056, 2019-04-03 (annotated)
- Committer:
- aingks
- Date:
- Wed Apr 03 02:49:34 2019 +0000
- Revision:
- 14:25241ed5b056
- Parent:
- 13:822b0e56ea68
- Child:
- 15:2a0cadb9b9dc
mail queue code added not tested;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aingks | 0:0943f040009c | 1 | #include "header.h" |
aingks | 3:f88d667629e6 | 2 | |
aingks | 2:9d6d07c9cc9c | 3 | |
aingks | 4:27340b291c09 | 4 | //definitions of tasks |
aingks | 4:27340b291c09 | 5 | //lock and unlock variables in struct when performing calculations |
aingks | 0:0943f040009c | 6 | |
aingks | 0:0943f040009c | 7 | //pedals |
aingks | 0:0943f040009c | 8 | AnalogIn brake_pedal(pin_analog_1); |
aingks | 0:0943f040009c | 9 | AnalogIn accel_pedal(pin_analog_2); |
aingks | 0:0943f040009c | 10 | |
aingks | 0:0943f040009c | 11 | |
aingks | 0:0943f040009c | 12 | //switches |
aingks | 0:0943f040009c | 13 | DigitalIn ngene(pin_digital_1); |
aingks | 1:c4f62ef9b5b7 | 14 | DigitalIn leftsw(pin_digital_2); |
aingks | 1:c4f62ef9b5b7 | 15 | DigitalIn rightsw(pin_digital_3); |
aingks | 1:c4f62ef9b5b7 | 16 | DigitalIn sidesw(pin_digital_4); |
aingks | 0:0943f040009c | 17 | |
aingks | 0:0943f040009c | 18 | |
aingks | 0:0943f040009c | 19 | //indicators |
aingks | 0:0943f040009c | 20 | DigitalOut ngeneind(pin_LED1); |
aingks | 13:822b0e56ea68 | 21 | DigitalOut leftind(pin_LED2); //was digitalout |
aingks | 13:822b0e56ea68 | 22 | DigitalOut rightind(pin_LED3);//was digitalout |
aingks | 0:0943f040009c | 23 | DigitalOut sideind(pin_LED4); |
aingks | 0:0943f040009c | 24 | |
aingks | 0:0943f040009c | 25 | |
aingks | 0:0943f040009c | 26 | //RED box lights |
aingks | 0:0943f040009c | 27 | DigitalOut brakeind(pin_LED5); |
aingks | 0:0943f040009c | 28 | DigitalOut overspeedind(pin_LED6); |
aingks | 0:0943f040009c | 29 | |
aingks | 4:27340b291c09 | 30 | |
aingks | 4:27340b291c09 | 31 | |
aingks | 4:27340b291c09 | 32 | // |
aingks | 4:27340b291c09 | 33 | extern SDATA speed; |
aingks | 4:27340b291c09 | 34 | extern CARINFO info; |
aingks | 4:27340b291c09 | 35 | extern SIMU_DATA sim; |
aingks | 4:27340b291c09 | 36 | |
aingks | 4:27340b291c09 | 37 | // LCD |
aingks | 4:27340b291c09 | 38 | |
aingks | 4:27340b291c09 | 39 | extern WattBob_TextLCD *display; |
aingks | 4:27340b291c09 | 40 | |
aingks | 4:27340b291c09 | 41 | // MUTEX |
aingks | 4:27340b291c09 | 42 | |
aingks | 4:27340b291c09 | 43 | extern Mutex simuXS; |
aingks | 4:27340b291c09 | 44 | extern Mutex speedXS; |
aingks | 4:27340b291c09 | 45 | extern Mutex carstateXS; |
aingks | 14:25241ed5b056 | 46 | |
aingks | 14:25241ed5b056 | 47 | |
aingks | 14:25241ed5b056 | 48 | extern Mail<SIMU_DATA, 16> mail_box; |
aingks | 14:25241ed5b056 | 49 | |
aingks | 14:25241ed5b056 | 50 | |
aingks | 2:9d6d07c9cc9c | 51 | |
aingks | 11:d6d1583fc824 | 52 | //serial port |
aingks | 11:d6d1583fc824 | 53 | Serial PC(USBTX, USBRX); |
aingks | 0:0943f040009c | 54 | |
aingks | 0:0943f040009c | 55 | void task1readbrake() |
aingks | 6:abbd51d9b045 | 56 | { |
aingks | 6:abbd51d9b045 | 57 | float delay = 1000 / TASKFREQ1; |
aingks | 6:abbd51d9b045 | 58 | while(true){ |
aingks | 6:abbd51d9b045 | 59 | |
aingks | 4:27340b291c09 | 60 | speedXS.lock();//lock |
aingks | 6:abbd51d9b045 | 61 | |
aingks | 1:c4f62ef9b5b7 | 62 | speed.brakevalue = brake_pedal.read(); |
aingks | 6:abbd51d9b045 | 63 | |
aingks | 4:27340b291c09 | 64 | speedXS.unlock();//unlock |
aingks | 6:abbd51d9b045 | 65 | |
aingks | 6:abbd51d9b045 | 66 | Thread::wait( (int) delay); |
aingks | 6:abbd51d9b045 | 67 | |
aingks | 6:abbd51d9b045 | 68 | } |
aingks | 0:0943f040009c | 69 | } |
aingks | 0:0943f040009c | 70 | |
aingks | 0:0943f040009c | 71 | void task2readaccel() |
aingks | 0:0943f040009c | 72 | { |
aingks | 6:abbd51d9b045 | 73 | float delay = 1000 / TASKFREQ2; |
aingks | 6:abbd51d9b045 | 74 | while(true){ |
aingks | 6:abbd51d9b045 | 75 | |
aingks | 4:27340b291c09 | 76 | speedXS.lock();//lock |
aingks | 6:abbd51d9b045 | 77 | |
aingks | 1:c4f62ef9b5b7 | 78 | speed.accelvalue = accel_pedal.read(); |
aingks | 6:abbd51d9b045 | 79 | |
aingks | 4:27340b291c09 | 80 | speedXS.unlock();//unlock |
aingks | 6:abbd51d9b045 | 81 | |
aingks | 6:abbd51d9b045 | 82 | Thread::wait( (int) delay); |
aingks | 6:abbd51d9b045 | 83 | |
aingks | 6:abbd51d9b045 | 84 | } |
aingks | 6:abbd51d9b045 | 85 | |
aingks | 0:0943f040009c | 86 | } |
aingks | 0:0943f040009c | 87 | |
aingks | 0:0943f040009c | 88 | void task3enginestate() |
aingks | 6:abbd51d9b045 | 89 | { |
aingks | 8:3f3f2c2e2046 | 90 | float delay = 125; |
aingks | 6:abbd51d9b045 | 91 | while(true){ |
aingks | 1:c4f62ef9b5b7 | 92 | //lock |
aingks | 4:27340b291c09 | 93 | carstateXS.lock(); |
aingks | 1:c4f62ef9b5b7 | 94 | uint8_t engineonoff = ngene.read(); |
aingks | 2:9d6d07c9cc9c | 95 | info.ENGINESTATE = engineonoff; |
aingks | 4:27340b291c09 | 96 | carstateXS.unlock(); |
aingks | 1:c4f62ef9b5b7 | 97 | //unlock |
aingks | 1:c4f62ef9b5b7 | 98 | |
aingks | 1:c4f62ef9b5b7 | 99 | if(engineonoff == 1){ |
aingks | 1:c4f62ef9b5b7 | 100 | ngeneind = 1; |
aingks | 1:c4f62ef9b5b7 | 101 | } |
aingks | 1:c4f62ef9b5b7 | 102 | else{ |
aingks | 1:c4f62ef9b5b7 | 103 | ngeneind = 0; |
aingks | 1:c4f62ef9b5b7 | 104 | } |
aingks | 6:abbd51d9b045 | 105 | |
aingks | 8:3f3f2c2e2046 | 106 | Thread::wait(125); |
aingks | 6:abbd51d9b045 | 107 | } |
aingks | 0:0943f040009c | 108 | } |
aingks | 0:0943f040009c | 109 | |
aingks | 0:0943f040009c | 110 | void task4average() |
aingks | 0:0943f040009c | 111 | { |
aingks | 6:abbd51d9b045 | 112 | float delay = 1000 / TASKFREQ4; |
aingks | 6:abbd51d9b045 | 113 | while(true){ |
aingks | 6:abbd51d9b045 | 114 | speedXS.lock();//lock |
aingks | 0:0943f040009c | 115 | |
aingks | 12:f4052a23ddc7 | 116 | for(int i = 0; i < SAMPLESIZE - 1 ; i++){ |
aingks | 7:87f7ba90fdce | 117 | //speed.rawspeed = (speed.accelvalue - speed.brakevalue)*MAXSPEED; |
aingks | 5:f5dda79b93cb | 118 | speed.average += speed.array[i]; |
aingks | 8:3f3f2c2e2046 | 119 | } |
aingks | 6:abbd51d9b045 | 120 | speed.average = (speed.average / SAMPLESIZE); |
aingks | 6:abbd51d9b045 | 121 | |
aingks | 6:abbd51d9b045 | 122 | speedXS.unlock();//unlock |
aingks | 6:abbd51d9b045 | 123 | |
aingks | 6:abbd51d9b045 | 124 | Thread::wait( (int) delay); |
aingks | 6:abbd51d9b045 | 125 | } |
aingks | 6:abbd51d9b045 | 126 | } |
aingks | 6:abbd51d9b045 | 127 | |
aingks | 6:abbd51d9b045 | 128 | void task5brakeLED() |
aingks | 6:abbd51d9b045 | 129 | { |
aingks | 6:abbd51d9b045 | 130 | float delay = 1000 / TASKFREQ5; |
aingks | 6:abbd51d9b045 | 131 | while(true){ |
aingks | 5:f5dda79b93cb | 132 | |
aingks | 6:abbd51d9b045 | 133 | //lock |
aingks | 6:abbd51d9b045 | 134 | speedXS.lock(); |
aingks | 6:abbd51d9b045 | 135 | float braketest = speed.brakevalue; |
aingks | 6:abbd51d9b045 | 136 | |
aingks | 6:abbd51d9b045 | 137 | speedXS.unlock();//unlock |
aingks | 6:abbd51d9b045 | 138 | |
aingks | 6:abbd51d9b045 | 139 | if( braketest > 0.2) // speed increase |
aingks | 6:abbd51d9b045 | 140 | brakeind = 1; |
aingks | 6:abbd51d9b045 | 141 | else |
aingks | 6:abbd51d9b045 | 142 | brakeind = 0; |
aingks | 5:f5dda79b93cb | 143 | |
aingks | 6:abbd51d9b045 | 144 | Thread::wait((int)delay); |
aingks | 6:abbd51d9b045 | 145 | } |
aingks | 7:87f7ba90fdce | 146 | } |
aingks | 7:87f7ba90fdce | 147 | |
aingks | 7:87f7ba90fdce | 148 | |
aingks | 7:87f7ba90fdce | 149 | void task6speedmonitor() |
aingks | 7:87f7ba90fdce | 150 | { |
aingks | 7:87f7ba90fdce | 151 | float delay = 1000 / TASKFREQ6; |
aingks | 7:87f7ba90fdce | 152 | while(true){ |
aingks | 7:87f7ba90fdce | 153 | |
aingks | 7:87f7ba90fdce | 154 | carstateXS.lock(); //lock carstate |
aingks | 7:87f7ba90fdce | 155 | speedXS.lock(); //lock speed |
aingks | 7:87f7ba90fdce | 156 | |
aingks | 7:87f7ba90fdce | 157 | |
aingks | 7:87f7ba90fdce | 158 | info.odometer += (speed.rawspeed / TASKFREQ6); |
aingks | 7:87f7ba90fdce | 159 | uint8_t temp_enginestate = info.ENGINESTATE; |
aingks | 7:87f7ba90fdce | 160 | |
aingks | 7:87f7ba90fdce | 161 | carstateXS.unlock(); //unlock carstate |
aingks | 7:87f7ba90fdce | 162 | |
aingks | 7:87f7ba90fdce | 163 | float temp_accel = 0.0; |
aingks | 7:87f7ba90fdce | 164 | if(temp_enginestate != 0) |
aingks | 7:87f7ba90fdce | 165 | temp_accel = speed.accelvalue; |
aingks | 7:87f7ba90fdce | 166 | |
aingks | 7:87f7ba90fdce | 167 | //update speed |
aingks | 7:87f7ba90fdce | 168 | speed.rawspeed += (temp_accel - speed.brakevalue) * MAXSPEED; |
aingks | 7:87f7ba90fdce | 169 | |
aingks | 7:87f7ba90fdce | 170 | if(speed.rawspeed < 0) |
aingks | 12:f4052a23ddc7 | 171 | speed.rawspeed = 0; |
aingks | 7:87f7ba90fdce | 172 | if(speed.rawspeed > MAXSPEED) |
aingks | 12:f4052a23ddc7 | 173 | speed.rawspeed = MAXSPEED; |
aingks | 7:87f7ba90fdce | 174 | |
aingks | 7:87f7ba90fdce | 175 | // Saving the new speed |
aingks | 7:87f7ba90fdce | 176 | for (int i = 0; i < SAMPLESIZE - 1; i++) |
aingks | 12:f4052a23ddc7 | 177 | speed.array[i]= speed.array[i+1]; |
aingks | 12:f4052a23ddc7 | 178 | speed.array[4]= speed.rawspeed; |
aingks | 7:87f7ba90fdce | 179 | |
aingks | 7:87f7ba90fdce | 180 | float temp = speed.rawspeed; |
aingks | 7:87f7ba90fdce | 181 | |
aingks | 7:87f7ba90fdce | 182 | // UNLOCK Speed |
aingks | 7:87f7ba90fdce | 183 | speedXS.unlock(); |
aingks | 7:87f7ba90fdce | 184 | |
aingks | 7:87f7ba90fdce | 185 | // Update Overspeed display |
aingks | 7:87f7ba90fdce | 186 | |
aingks | 12:f4052a23ddc7 | 187 | if (temp > 39.3395) //88 mph to ms |
aingks | 7:87f7ba90fdce | 188 | overspeedind = 1; |
aingks | 7:87f7ba90fdce | 189 | else |
aingks | 7:87f7ba90fdce | 190 | overspeedind = 0; |
aingks | 7:87f7ba90fdce | 191 | |
aingks | 7:87f7ba90fdce | 192 | Thread::wait((int)delay); |
aingks | 7:87f7ba90fdce | 193 | } |
aingks | 7:87f7ba90fdce | 194 | } |
aingks | 7:87f7ba90fdce | 195 | |
aingks | 7:87f7ba90fdce | 196 | void task7displayvalues() |
aingks | 7:87f7ba90fdce | 197 | { |
aingks | 7:87f7ba90fdce | 198 | float delay = 1000 / TASKFREQ7; |
aingks | 12:f4052a23ddc7 | 199 | display->cls(); |
aingks | 7:87f7ba90fdce | 200 | while(true){ |
aingks | 8:3f3f2c2e2046 | 201 | |
aingks | 12:f4052a23ddc7 | 202 | |
aingks | 7:87f7ba90fdce | 203 | |
aingks | 7:87f7ba90fdce | 204 | //LOCK data |
aingks | 7:87f7ba90fdce | 205 | carstateXS.lock(); |
aingks | 7:87f7ba90fdce | 206 | //LOCK Speed |
aingks | 7:87f7ba90fdce | 207 | speedXS.lock(); |
aingks | 7:87f7ba90fdce | 208 | |
aingks | 7:87f7ba90fdce | 209 | display->locate(1,0); |
aingks | 12:f4052a23ddc7 | 210 | display->printf("AvgS: %.2f m/s",speed.average); |
aingks | 7:87f7ba90fdce | 211 | display->locate(0,0); |
aingks | 12:f4052a23ddc7 | 212 | display->printf("ODOM: %d m",(int) info.odometer); |
aingks | 7:87f7ba90fdce | 213 | |
aingks | 7:87f7ba90fdce | 214 | //UNLOCK data |
aingks | 7:87f7ba90fdce | 215 | carstateXS.unlock(); |
aingks | 7:87f7ba90fdce | 216 | //UNLOCK Speed |
aingks | 7:87f7ba90fdce | 217 | speedXS.unlock(); |
aingks | 7:87f7ba90fdce | 218 | |
aingks | 8:3f3f2c2e2046 | 219 | |
aingks | 8:3f3f2c2e2046 | 220 | |
aingks | 8:3f3f2c2e2046 | 221 | |
aingks | 14:25241ed5b056 | 222 | Thread::wait(delay); |
aingks | 8:3f3f2c2e2046 | 223 | |
aingks | 8:3f3f2c2e2046 | 224 | |
aingks | 8:3f3f2c2e2046 | 225 | } |
aingks | 8:3f3f2c2e2046 | 226 | } |
aingks | 8:3f3f2c2e2046 | 227 | |
aingks | 8:3f3f2c2e2046 | 228 | void task8sidelights() |
aingks | 8:3f3f2c2e2046 | 229 | { |
aingks | 8:3f3f2c2e2046 | 230 | float delay = 1000 / TASKFREQ8; |
aingks | 8:3f3f2c2e2046 | 231 | while(true){ |
aingks | 8:3f3f2c2e2046 | 232 | |
aingks | 8:3f3f2c2e2046 | 233 | // LOCK Car data |
aingks | 8:3f3f2c2e2046 | 234 | carstateXS.lock(); |
aingks | 8:3f3f2c2e2046 | 235 | |
aingks | 8:3f3f2c2e2046 | 236 | info.sideind = sidesw.read(); |
aingks | 8:3f3f2c2e2046 | 237 | uint8_t temp = info.sideind; |
aingks | 8:3f3f2c2e2046 | 238 | |
aingks | 8:3f3f2c2e2046 | 239 | //UNLOCK Car data |
aingks | 8:3f3f2c2e2046 | 240 | carstateXS.unlock(); |
aingks | 8:3f3f2c2e2046 | 241 | |
aingks | 8:3f3f2c2e2046 | 242 | if (temp == 1) |
aingks | 8:3f3f2c2e2046 | 243 | sideind = 1; |
aingks | 8:3f3f2c2e2046 | 244 | else |
aingks | 8:3f3f2c2e2046 | 245 | sideind = 0; |
aingks | 8:3f3f2c2e2046 | 246 | |
aingks | 7:87f7ba90fdce | 247 | Thread::wait((int)delay); |
aingks | 7:87f7ba90fdce | 248 | |
aingks | 7:87f7ba90fdce | 249 | |
aingks | 8:3f3f2c2e2046 | 250 | |
aingks | 9:2fd97246b8f0 | 251 | |
aingks | 7:87f7ba90fdce | 252 | } |
aingks | 9:2fd97246b8f0 | 253 | } |
aingks | 12:f4052a23ddc7 | 254 | |
aingks | 9:2fd97246b8f0 | 255 | void task9indLED() |
aingks | 9:2fd97246b8f0 | 256 | { |
aingks | 13:822b0e56ea68 | 257 | int count = 0; |
aingks | 9:2fd97246b8f0 | 258 | while(true){ |
aingks | 9:2fd97246b8f0 | 259 | |
aingks | 10:2b262d810c67 | 260 | |
aingks | 13:822b0e56ea68 | 261 | /* int turnleft = leftsw; |
aingks | 10:2b262d810c67 | 262 | int turnright = rightsw; |
aingks | 9:2fd97246b8f0 | 263 | |
aingks | 9:2fd97246b8f0 | 264 | |
aingks | 9:2fd97246b8f0 | 265 | |
aingks | 10:2b262d810c67 | 266 | carstateXS.lock(); |
aingks | 10:2b262d810c67 | 267 | |
aingks | 12:f4052a23ddc7 | 268 | (turnleft == 1)? info.leftind = 1 :info.leftind = 0; |
aingks | 12:f4052a23ddc7 | 269 | (turnright == 1)? info.rightind = 1 :info.rightind = 0; |
aingks | 9:2fd97246b8f0 | 270 | |
aingks | 10:2b262d810c67 | 271 | carstateXS.unlock(); |
aingks | 11:d6d1583fc824 | 272 | |
aingks | 9:2fd97246b8f0 | 273 | |
aingks | 10:2b262d810c67 | 274 | |
aingks | 9:2fd97246b8f0 | 275 | |
aingks | 9:2fd97246b8f0 | 276 | //hazard and other lights |
aingks | 9:2fd97246b8f0 | 277 | |
aingks | 9:2fd97246b8f0 | 278 | float left_period = 1.0; |
aingks | 9:2fd97246b8f0 | 279 | float right_period = 1.0; |
aingks | 9:2fd97246b8f0 | 280 | float left_pulswidth = 0.0; |
aingks | 13:822b0e56ea68 | 281 | float right_pulswidth = 0.0;*/ |
aingks | 9:2fd97246b8f0 | 282 | |
aingks | 13:822b0e56ea68 | 283 | /*if (turnleft != 0) |
aingks | 9:2fd97246b8f0 | 284 | { |
aingks | 9:2fd97246b8f0 | 285 | left_period = (1/TASKFREQ9); |
aingks | 9:2fd97246b8f0 | 286 | left_pulswidth = (1/(2*TASKFREQ9)); |
aingks | 9:2fd97246b8f0 | 287 | } |
aingks | 9:2fd97246b8f0 | 288 | if (turnright!= 0) |
aingks | 9:2fd97246b8f0 | 289 | { |
aingks | 9:2fd97246b8f0 | 290 | right_period = (1/TASKFREQ9); |
aingks | 9:2fd97246b8f0 | 291 | right_pulswidth = (1/TASKFREQ9); |
aingks | 9:2fd97246b8f0 | 292 | } |
aingks | 9:2fd97246b8f0 | 293 | if (turnleft != 0 && turnright != 0) |
aingks | 9:2fd97246b8f0 | 294 | { |
aingks | 9:2fd97246b8f0 | 295 | left_period = left_period / 2; |
aingks | 9:2fd97246b8f0 | 296 | left_pulswidth = left_pulswidth / 2; |
aingks | 9:2fd97246b8f0 | 297 | right_period = right_period / 2; |
aingks | 9:2fd97246b8f0 | 298 | right_pulswidth = right_pulswidth / 2; |
aingks | 9:2fd97246b8f0 | 299 | } |
aingks | 9:2fd97246b8f0 | 300 | |
aingks | 10:2b262d810c67 | 301 | |
aingks | 9:2fd97246b8f0 | 302 | leftind.period(left_period); |
aingks | 9:2fd97246b8f0 | 303 | leftind.pulsewidth(left_pulswidth); |
aingks | 9:2fd97246b8f0 | 304 | rightind.period(right_period); |
aingks | 9:2fd97246b8f0 | 305 | rightind.pulsewidth(right_pulswidth); |
aingks | 13:822b0e56ea68 | 306 | */ |
aingks | 10:2b262d810c67 | 307 | |
aingks | 10:2b262d810c67 | 308 | |
aingks | 9:2fd97246b8f0 | 309 | |
aingks | 13:822b0e56ea68 | 310 | //float delay = 1000 / TASKFREQ9S2; |
aingks | 13:822b0e56ea68 | 311 | //Thread::wait((int)delay); |
aingks | 13:822b0e56ea68 | 312 | |
aingks | 13:822b0e56ea68 | 313 | // Counter ensure everything works off of 2Hz loop |
aingks | 13:822b0e56ea68 | 314 | count++; |
aingks | 13:822b0e56ea68 | 315 | leftind = leftsw; |
aingks | 13:822b0e56ea68 | 316 | rightind = rightsw; |
aingks | 13:822b0e56ea68 | 317 | |
aingks | 13:822b0e56ea68 | 318 | if (leftsw == 1 && rightsw == 1){ |
aingks | 14:25241ed5b056 | 319 | carstateXS.lock(); |
aingks | 14:25241ed5b056 | 320 | leftind = 0; |
aingks | 14:25241ed5b056 | 321 | rightind = 0; // current issue :just stopping here |
aingks | 14:25241ed5b056 | 322 | carstateXS.unlock(); |
aingks | 13:822b0e56ea68 | 323 | } |
aingks | 13:822b0e56ea68 | 324 | if (count == 2||count == 4){ // flash indicators every second |
aingks | 13:822b0e56ea68 | 325 | carstateXS.lock(); |
aingks | 13:822b0e56ea68 | 326 | if (leftind == 1 && rightind == 0){ |
aingks | 13:822b0e56ea68 | 327 | leftind = 0; |
aingks | 13:822b0e56ea68 | 328 | } |
aingks | 13:822b0e56ea68 | 329 | else if (rightind == 1 && leftind == 0){ |
aingks | 13:822b0e56ea68 | 330 | rightind = 0; |
aingks | 13:822b0e56ea68 | 331 | } |
aingks | 9:2fd97246b8f0 | 332 | |
aingks | 13:822b0e56ea68 | 333 | else if (leftind == 0 && rightind == 0){ |
aingks | 13:822b0e56ea68 | 334 | } |
aingks | 13:822b0e56ea68 | 335 | carstateXS.unlock(); |
aingks | 13:822b0e56ea68 | 336 | } |
aingks | 13:822b0e56ea68 | 337 | if (count == 4){ // send data every 2 seconds |
aingks | 13:822b0e56ea68 | 338 | count = 0; |
aingks | 13:822b0e56ea68 | 339 | } |
aingks | 13:822b0e56ea68 | 340 | Thread::wait(500); |
aingks | 9:2fd97246b8f0 | 341 | |
aingks | 9:2fd97246b8f0 | 342 | } |
aingks | 9:2fd97246b8f0 | 343 | |
aingks | 13:822b0e56ea68 | 344 | } |
aingks | 9:2fd97246b8f0 | 345 | |
aingks | 9:2fd97246b8f0 | 346 | |
aingks | 10:2b262d810c67 | 347 | void taskXserialdump() |
aingks | 10:2b262d810c67 | 348 | { |
aingks | 10:2b262d810c67 | 349 | while(true) |
aingks | 10:2b262d810c67 | 350 | { |
aingks | 14:25241ed5b056 | 351 | speedXS.lock(); |
aingks | 14:25241ed5b056 | 352 | |
aingks | 14:25241ed5b056 | 353 | PC.printf("SPEED: %.1f\r\n",speed.rawspeed); |
aingks | 14:25241ed5b056 | 354 | PC.printf("BRAKE: %.1f\r\n",speed.brakevalue); |
aingks | 14:25241ed5b056 | 355 | PC.printf("ACCELEROMETER: %.1f\r\n",speed.accelvalue); |
aingks | 14:25241ed5b056 | 356 | |
aingks | 14:25241ed5b056 | 357 | speedXS.unlock(); |
aingks | 14:25241ed5b056 | 358 | |
aingks | 14:25241ed5b056 | 359 | carstateXS.lock(); |
aingks | 14:25241ed5b056 | 360 | |
aingks | 14:25241ed5b056 | 361 | PC.printf("LEFT INDICATOR : %.1f\r\n",info.leftind); |
aingks | 14:25241ed5b056 | 362 | PC.printf("RIGHT INDICATOR : %.1f\r\n",info.rightind); |
aingks | 14:25241ed5b056 | 363 | |
aingks | 14:25241ed5b056 | 364 | carstateXS.unlock(); |
aingks | 14:25241ed5b056 | 365 | |
aingks | 14:25241ed5b056 | 366 | float delay = 1000/ TASKFREQX; |
aingks | 14:25241ed5b056 | 367 | Thread:: wait((int)delay); |
aingks | 10:2b262d810c67 | 368 | |
aingks | 10:2b262d810c67 | 369 | } |
aingks | 10:2b262d810c67 | 370 | |
aingks | 10:2b262d810c67 | 371 | |
aingks | 10:2b262d810c67 | 372 | |
aingks | 10:2b262d810c67 | 373 | } |
aingks | 14:25241ed5b056 | 374 | |
aingks | 14:25241ed5b056 | 375 | |
aingks | 14:25241ed5b056 | 376 | //mail queue |
aingks | 14:25241ed5b056 | 377 | void send_thread (void) { |
aingks | 14:25241ed5b056 | 378 | |
aingks | 14:25241ed5b056 | 379 | while (true) { |
aingks | 14:25241ed5b056 | 380 | |
aingks | 14:25241ed5b056 | 381 | SIMU_DATA *mail = mail_box.alloc(); |
aingks | 14:25241ed5b056 | 382 | |
aingks | 14:25241ed5b056 | 383 | speedXS.lock(); |
aingks | 14:25241ed5b056 | 384 | mail->brakevalue = speed.brakevalue; |
aingks | 14:25241ed5b056 | 385 | mail->accelvalue = speed.accelvalue; |
aingks | 14:25241ed5b056 | 386 | mail->rawspeed = speed.rawspeed; |
aingks | 14:25241ed5b056 | 387 | speedXS.unlock(); |
aingks | 14:25241ed5b056 | 388 | |
aingks | 14:25241ed5b056 | 389 | carstateXS.lock(); |
aingks | 14:25241ed5b056 | 390 | mail->odometer = info.odometer; |
aingks | 14:25241ed5b056 | 391 | carstateXS.unlock(); |
aingks | 14:25241ed5b056 | 392 | |
aingks | 14:25241ed5b056 | 393 | mail_box.put(mail); |
aingks | 14:25241ed5b056 | 394 | |
aingks | 14:25241ed5b056 | 395 | //float delay = 1000/ MailQueue; |
aingks | 14:25241ed5b056 | 396 | Thread::wait(1000); |
aingks | 14:25241ed5b056 | 397 | } |
aingks | 14:25241ed5b056 | 398 | } |
aingks | 14:25241ed5b056 | 399 | |
aingks | 12:f4052a23ddc7 | 400 | //////////////////////////////////// |
aingks | 14:25241ed5b056 | 401 | |
aingks | 14:25241ed5b056 | 402 | |
aingks | 14:25241ed5b056 | 403 | |
aingks | 14:25241ed5b056 | 404 | |
aingks | 14:25241ed5b056 | 405 | |
aingks | 14:25241ed5b056 | 406 | |
aingks | 14:25241ed5b056 | 407 | |
aingks | 14:25241ed5b056 | 408 | |
aingks | 14:25241ed5b056 | 409 | |
aingks | 14:25241ed5b056 | 410 | |
aingks | 14:25241ed5b056 | 411 | |
aingks | 12:f4052a23ddc7 | 412 | /*void IndicatorsCheck(){ |
aingks | 12:f4052a23ddc7 | 413 | LeftIndicatorLight = LeftIndicatorSwitch; |
aingks | 12:f4052a23ddc7 | 414 | RightIndicatorLight = RightIndicatorSwitch; |
aingks | 12:f4052a23ddc7 | 415 | } |
aingks | 12:f4052a23ddc7 | 416 | // Flash respective indicator every 1 second// linked from above |
aingks | 12:f4052a23ddc7 | 417 | void IndicatorFlash(){ |
aingks | 12:f4052a23ddc7 | 418 | Global_Sem.wait(); |
aingks | 12:f4052a23ddc7 | 419 | if (LeftIndicatorLight == 1 && RightIndicatorLight == 0){ |
aingks | 12:f4052a23ddc7 | 420 | LeftIndicatorLight = 0; |
aingks | 12:f4052a23ddc7 | 421 | } |
aingks | 12:f4052a23ddc7 | 422 | else if (RightIndicatorLight == 1 && LeftIndicatorLight == 0){ |
aingks | 12:f4052a23ddc7 | 423 | RightIndicatorLight = 0; |
aingks | 12:f4052a23ddc7 | 424 | } |
aingks | 12:f4052a23ddc7 | 425 | else if (LeftIndicatorLight == 0 && RightIndicatorLight == 0){ |
aingks | 12:f4052a23ddc7 | 426 | } |
aingks | 12:f4052a23ddc7 | 427 | Global_Sem.release(); |
aingks | 12:f4052a23ddc7 | 428 | } |
aingks | 12:f4052a23ddc7 | 429 | // Flash hazards every o.5 seconds |
aingks | 12:f4052a23ddc7 | 430 | void HazardLights(){ |
aingks | 12:f4052a23ddc7 | 431 | Global_Sem.wait(); |
aingks | 12:f4052a23ddc7 | 432 | if (LeftIndicatorSwitch == 1 && RightIndicatorSwitch == 1){ |
aingks | 12:f4052a23ddc7 | 433 | LeftIndicatorLight = 0; |
aingks | 12:f4052a23ddc7 | 434 | RightIndicatorLight = 0; |
aingks | 12:f4052a23ddc7 | 435 | } |
aingks | 12:f4052a23ddc7 | 436 | Global_Sem.release(); |
aingks | 12:f4052a23ddc7 | 437 | } |
aingks | 12:f4052a23ddc7 | 438 | void TwoHzLoop(){ |
aingks | 12:f4052a23ddc7 | 439 | while(true){ |
aingks | 12:f4052a23ddc7 | 440 | |
aingks | 12:f4052a23ddc7 | 441 | count++; // Counter ensure everything works off of 2Hz loop |
aingks | 12:f4052a23ddc7 | 442 | Display(); |
aingks | 12:f4052a23ddc7 | 443 | IndicatorsCheck(); |
aingks | 12:f4052a23ddc7 | 444 | HazardLights(); |
aingks | 12:f4052a23ddc7 | 445 | if (count == 2||count == 4){ // flash indicators every second |
aingks | 12:f4052a23ddc7 | 446 | IndicatorFlash(); |
aingks | 12:f4052a23ddc7 | 447 | } |
aingks | 12:f4052a23ddc7 | 448 | if (count == 4){ // send data every 2 seconds |
aingks | 12:f4052a23ddc7 | 449 | SendData(); |
aingks | 12:f4052a23ddc7 | 450 | count = 0; |
aingks | 12:f4052a23ddc7 | 451 | } |
aingks | 12:f4052a23ddc7 | 452 | Thread::wait(500); |
aingks | 12:f4052a23ddc7 | 453 | } |
aingks | 12:f4052a23ddc7 | 454 | } |
aingks | 12:f4052a23ddc7 | 455 | */ |