DP
Dependencies: FastAnalogIn mbed-rtos mbed
threads.cpp@0:f3b355df6f26, 2015-04-26 (annotated)
- Committer:
- romankrej
- Date:
- Sun Apr 26 13:14:02 2015 +0000
- Revision:
- 0:f3b355df6f26
Diplomova prace 2015
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
romankrej | 0:f3b355df6f26 | 1 | #include "threads.h" |
romankrej | 0:f3b355df6f26 | 2 | |
romankrej | 0:f3b355df6f26 | 3 | /* |
romankrej | 0:f3b355df6f26 | 4 | * LED diods |
romankrej | 0:f3b355df6f26 | 5 | */ |
romankrej | 0:f3b355df6f26 | 6 | DigitalOut ledMode(LED1); |
romankrej | 0:f3b355df6f26 | 7 | DigitalOut ledSensor(LED2); |
romankrej | 0:f3b355df6f26 | 8 | DigitalOut ledController(LED3); |
romankrej | 0:f3b355df6f26 | 9 | |
romankrej | 0:f3b355df6f26 | 10 | |
romankrej | 0:f3b355df6f26 | 11 | /* |
romankrej | 0:f3b355df6f26 | 12 | * Buttons |
romankrej | 0:f3b355df6f26 | 13 | */ |
romankrej | 0:f3b355df6f26 | 14 | DigitalIn swSensor(p7); |
romankrej | 0:f3b355df6f26 | 15 | DigitalIn swController(p8); |
romankrej | 0:f3b355df6f26 | 16 | InterruptIn butOff(p5); |
romankrej | 0:f3b355df6f26 | 17 | |
romankrej | 0:f3b355df6f26 | 18 | |
romankrej | 0:f3b355df6f26 | 19 | /* |
romankrej | 0:f3b355df6f26 | 20 | * Instances of classes |
romankrej | 0:f3b355df6f26 | 21 | */ |
romankrej | 0:f3b355df6f26 | 22 | cUltrasonic us100(p12,p13); |
romankrej | 0:f3b355df6f26 | 23 | cCan can(p30,p29); |
romankrej | 0:f3b355df6f26 | 24 | cPrint pc(USBTX,USBRX); |
romankrej | 0:f3b355df6f26 | 25 | cLedSensor baumer(p20); |
romankrej | 0:f3b355df6f26 | 26 | cControl control; |
romankrej | 0:f3b355df6f26 | 27 | |
romankrej | 0:f3b355df6f26 | 28 | |
romankrej | 0:f3b355df6f26 | 29 | char first; |
romankrej | 0:f3b355df6f26 | 30 | states_t states; |
romankrej | 0:f3b355df6f26 | 31 | Mutex mutex1; |
romankrej | 0:f3b355df6f26 | 32 | char programMode; |
romankrej | 0:f3b355df6f26 | 33 | char sensor; |
romankrej | 0:f3b355df6f26 | 34 | char controller; |
romankrej | 0:f3b355df6f26 | 35 | char stisknut; |
romankrej | 0:f3b355df6f26 | 36 | |
romankrej | 0:f3b355df6f26 | 37 | |
romankrej | 0:f3b355df6f26 | 38 | |
romankrej | 0:f3b355df6f26 | 39 | |
romankrej | 0:f3b355df6f26 | 40 | |
romankrej | 0:f3b355df6f26 | 41 | /* |
romankrej | 0:f3b355df6f26 | 42 | * ISR of stop button |
romankrej | 0:f3b355df6f26 | 43 | */ |
romankrej | 0:f3b355df6f26 | 44 | void stopProg() { |
romankrej | 0:f3b355df6f26 | 45 | __disable_irq(); |
romankrej | 0:f3b355df6f26 | 46 | stisknut = 1; |
romankrej | 0:f3b355df6f26 | 47 | can.disableServo(); |
romankrej | 0:f3b355df6f26 | 48 | programMode = STOP; |
romankrej | 0:f3b355df6f26 | 49 | __enable_irq(); |
romankrej | 0:f3b355df6f26 | 50 | } |
romankrej | 0:f3b355df6f26 | 51 | |
romankrej | 0:f3b355df6f26 | 52 | |
romankrej | 0:f3b355df6f26 | 53 | void initThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 54 | programMode = WAITING; |
romankrej | 0:f3b355df6f26 | 55 | sensor = swSensor.read(); |
romankrej | 0:f3b355df6f26 | 56 | controller = swController.read(); |
romankrej | 0:f3b355df6f26 | 57 | ledSensor = sensor; |
romankrej | 0:f3b355df6f26 | 58 | ledController = controller; |
romankrej | 0:f3b355df6f26 | 59 | if(sensor == ULTRA) { |
romankrej | 0:f3b355df6f26 | 60 | pc.log("Zadan ultrazvukovy snimac"); |
romankrej | 0:f3b355df6f26 | 61 | } |
romankrej | 0:f3b355df6f26 | 62 | else { |
romankrej | 0:f3b355df6f26 | 63 | pc.log("Zadan LED snimac"); |
romankrej | 0:f3b355df6f26 | 64 | } |
romankrej | 0:f3b355df6f26 | 65 | if(controller == ST_FEEDBACK) |
romankrej | 0:f3b355df6f26 | 66 | pc.log("Zadana stavova zpetna vazba"); |
romankrej | 0:f3b355df6f26 | 67 | else |
romankrej | 0:f3b355df6f26 | 68 | pc.log("Zadan PID regulator"); |
romankrej | 0:f3b355df6f26 | 69 | } |
romankrej | 0:f3b355df6f26 | 70 | |
romankrej | 0:f3b355df6f26 | 71 | |
romankrej | 0:f3b355df6f26 | 72 | /* |
romankrej | 0:f3b355df6f26 | 73 | * This thread indicate current state of program |
romankrej | 0:f3b355df6f26 | 74 | */ |
romankrej | 0:f3b355df6f26 | 75 | void ledThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 76 | while(true) { |
romankrej | 0:f3b355df6f26 | 77 | switch(programMode) { |
romankrej | 0:f3b355df6f26 | 78 | case WAITING: |
romankrej | 0:f3b355df6f26 | 79 | ledMode = 1; |
romankrej | 0:f3b355df6f26 | 80 | can.nullResolver(); |
romankrej | 0:f3b355df6f26 | 81 | break; |
romankrej | 0:f3b355df6f26 | 82 | case STARTING: |
romankrej | 0:f3b355df6f26 | 83 | ledMode = 1; |
romankrej | 0:f3b355df6f26 | 84 | break; |
romankrej | 0:f3b355df6f26 | 85 | case RUNNING: |
romankrej | 0:f3b355df6f26 | 86 | ledMode = !ledMode; |
romankrej | 0:f3b355df6f26 | 87 | break; |
romankrej | 0:f3b355df6f26 | 88 | case STOP: |
romankrej | 0:f3b355df6f26 | 89 | if(stisknut == 1) { |
romankrej | 0:f3b355df6f26 | 90 | pc.close(); |
romankrej | 0:f3b355df6f26 | 91 | stisknut = 0; |
romankrej | 0:f3b355df6f26 | 92 | } |
romankrej | 0:f3b355df6f26 | 93 | ledMode = 0; |
romankrej | 0:f3b355df6f26 | 94 | ledSensor = 0; |
romankrej | 0:f3b355df6f26 | 95 | ledController = 0; |
romankrej | 0:f3b355df6f26 | 96 | } |
romankrej | 0:f3b355df6f26 | 97 | if(sensor == ULTRA) |
romankrej | 0:f3b355df6f26 | 98 | ledSensor = !ledSensor; |
romankrej | 0:f3b355df6f26 | 99 | if(controller == PID) |
romankrej | 0:f3b355df6f26 | 100 | ledController = !ledController; |
romankrej | 0:f3b355df6f26 | 101 | Thread::wait(200); |
romankrej | 0:f3b355df6f26 | 102 | } |
romankrej | 0:f3b355df6f26 | 103 | |
romankrej | 0:f3b355df6f26 | 104 | } |
romankrej | 0:f3b355df6f26 | 105 | |
romankrej | 0:f3b355df6f26 | 106 | |
romankrej | 0:f3b355df6f26 | 107 | /* |
romankrej | 0:f3b355df6f26 | 108 | * Thread for ultrasonic sensor |
romankrej | 0:f3b355df6f26 | 109 | * This thread sets trigger every 15 ms |
romankrej | 0:f3b355df6f26 | 110 | */ |
romankrej | 0:f3b355df6f26 | 111 | void usThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 112 | while(true) { |
romankrej | 0:f3b355df6f26 | 113 | if(programMode != STOP) { |
romankrej | 0:f3b355df6f26 | 114 | can.actual_speed(); |
romankrej | 0:f3b355df6f26 | 115 | us100.setTrig(); |
romankrej | 0:f3b355df6f26 | 116 | } |
romankrej | 0:f3b355df6f26 | 117 | Thread::wait(15); |
romankrej | 0:f3b355df6f26 | 118 | } |
romankrej | 0:f3b355df6f26 | 119 | } |
romankrej | 0:f3b355df6f26 | 120 | |
romankrej | 0:f3b355df6f26 | 121 | /* |
romankrej | 0:f3b355df6f26 | 122 | * Thread for led sensor |
romankrej | 0:f3b355df6f26 | 123 | * This thread requires reading analog value every 3 ms |
romankrej | 0:f3b355df6f26 | 124 | */ |
romankrej | 0:f3b355df6f26 | 125 | void laserThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 126 | while(true) { |
romankrej | 0:f3b355df6f26 | 127 | can.actual_speed(); |
romankrej | 0:f3b355df6f26 | 128 | if(programMode != STOP) { |
romankrej | 0:f3b355df6f26 | 129 | baumer.read(); |
romankrej | 0:f3b355df6f26 | 130 | } |
romankrej | 0:f3b355df6f26 | 131 | Thread::wait(3); |
romankrej | 0:f3b355df6f26 | 132 | } |
romankrej | 0:f3b355df6f26 | 133 | } |
romankrej | 0:f3b355df6f26 | 134 | |
romankrej | 0:f3b355df6f26 | 135 | |
romankrej | 0:f3b355df6f26 | 136 | /* |
romankrej | 0:f3b355df6f26 | 137 | * This thread control system |
romankrej | 0:f3b355df6f26 | 138 | */ |
romankrej | 0:f3b355df6f26 | 139 | void controlThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 140 | while(true) { |
romankrej | 0:f3b355df6f26 | 141 | if(programMode != STOP) { |
romankrej | 0:f3b355df6f26 | 142 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 143 | control.setCurrent(); |
romankrej | 0:f3b355df6f26 | 144 | can.setCurrent(states.current); |
romankrej | 0:f3b355df6f26 | 145 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 146 | } |
romankrej | 0:f3b355df6f26 | 147 | Thread::wait(15); |
romankrej | 0:f3b355df6f26 | 148 | |
romankrej | 0:f3b355df6f26 | 149 | } |
romankrej | 0:f3b355df6f26 | 150 | } |
romankrej | 0:f3b355df6f26 | 151 | |
romankrej | 0:f3b355df6f26 | 152 | /* |
romankrej | 0:f3b355df6f26 | 153 | * This thread thread collect the states of system |
romankrej | 0:f3b355df6f26 | 154 | */ |
romankrej | 0:f3b355df6f26 | 155 | void collectThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 156 | while(true) { |
romankrej | 0:f3b355df6f26 | 157 | osEvent ev = Thread::signal_wait(0); |
romankrej | 0:f3b355df6f26 | 158 | if(programMode != STOP) { |
romankrej | 0:f3b355df6f26 | 159 | float timeout, volts,phi_temp,phi_old,omega_temp,phi,omega; |
romankrej | 0:f3b355df6f26 | 160 | if(ev.status == osEventSignal) { |
romankrej | 0:f3b355df6f26 | 161 | switch(ev.value.signals) { |
romankrej | 0:f3b355df6f26 | 162 | case 0x01 : |
romankrej | 0:f3b355df6f26 | 163 | if(sensor == ULTRA) { |
romankrej | 0:f3b355df6f26 | 164 | timeout = us100.getPulseWidth(); |
romankrej | 0:f3b355df6f26 | 165 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 166 | phi_old = states.phi1; |
romankrej | 0:f3b355df6f26 | 167 | phi = states.phi1; |
romankrej | 0:f3b355df6f26 | 168 | omega = states.omega1; |
romankrej | 0:f3b355df6f26 | 169 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 170 | |
romankrej | 0:f3b355df6f26 | 171 | if(timeout > 2000) { |
romankrej | 0:f3b355df6f26 | 172 | programMode = WAITING; |
romankrej | 0:f3b355df6f26 | 173 | first = 0; |
romankrej | 0:f3b355df6f26 | 174 | } |
romankrej | 0:f3b355df6f26 | 175 | |
romankrej | 0:f3b355df6f26 | 176 | else { |
romankrej | 0:f3b355df6f26 | 177 | if(programMode == WAITING) |
romankrej | 0:f3b355df6f26 | 178 | programMode = STARTING; |
romankrej | 0:f3b355df6f26 | 179 | } |
romankrej | 0:f3b355df6f26 | 180 | |
romankrej | 0:f3b355df6f26 | 181 | if(programMode == STARTING) |
romankrej | 0:f3b355df6f26 | 182 | first++; |
romankrej | 0:f3b355df6f26 | 183 | |
romankrej | 0:f3b355df6f26 | 184 | if(first > 120) |
romankrej | 0:f3b355df6f26 | 185 | programMode = RUNNING; |
romankrej | 0:f3b355df6f26 | 186 | |
romankrej | 0:f3b355df6f26 | 187 | if(programMode == RUNNING) { |
romankrej | 0:f3b355df6f26 | 188 | phi_temp = (float)(0.0005*timeout - 0.4925)+0.02; |
romankrej | 0:f3b355df6f26 | 189 | if(first > 120) { |
romankrej | 0:f3b355df6f26 | 190 | phi = phi_temp; |
romankrej | 0:f3b355df6f26 | 191 | phi_old = phi; |
romankrej | 0:f3b355df6f26 | 192 | } |
romankrej | 0:f3b355df6f26 | 193 | |
romankrej | 0:f3b355df6f26 | 194 | else { |
romankrej | 0:f3b355df6f26 | 195 | if((phi_temp > phi+0.15)||(phi_temp < phi-0.15)) { |
romankrej | 0:f3b355df6f26 | 196 | phi_temp = phi; |
romankrej | 0:f3b355df6f26 | 197 | } |
romankrej | 0:f3b355df6f26 | 198 | } |
romankrej | 0:f3b355df6f26 | 199 | |
romankrej | 0:f3b355df6f26 | 200 | phi = phi_temp; |
romankrej | 0:f3b355df6f26 | 201 | |
romankrej | 0:f3b355df6f26 | 202 | omega_temp = (phi - phi_old) * 66.67 ; |
romankrej | 0:f3b355df6f26 | 203 | if(first > 120) { |
romankrej | 0:f3b355df6f26 | 204 | omega = omega_temp; |
romankrej | 0:f3b355df6f26 | 205 | first = 0; |
romankrej | 0:f3b355df6f26 | 206 | } |
romankrej | 0:f3b355df6f26 | 207 | |
romankrej | 0:f3b355df6f26 | 208 | else { |
romankrej | 0:f3b355df6f26 | 209 | if((omega_temp > omega + 3.0 ) || (omega_temp < omega - 3.0)){ |
romankrej | 0:f3b355df6f26 | 210 | omega_temp = omega; |
romankrej | 0:f3b355df6f26 | 211 | } |
romankrej | 0:f3b355df6f26 | 212 | omega = 0.8 * omega + 0.2 * omega_temp; |
romankrej | 0:f3b355df6f26 | 213 | } |
romankrej | 0:f3b355df6f26 | 214 | } |
romankrej | 0:f3b355df6f26 | 215 | } |
romankrej | 0:f3b355df6f26 | 216 | |
romankrej | 0:f3b355df6f26 | 217 | else { |
romankrej | 0:f3b355df6f26 | 218 | |
romankrej | 0:f3b355df6f26 | 219 | volts = baumer.getVoltage(); |
romankrej | 0:f3b355df6f26 | 220 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 221 | phi_old = states.phi1; |
romankrej | 0:f3b355df6f26 | 222 | phi = states.phi1; |
romankrej | 0:f3b355df6f26 | 223 | omega = states.omega1; |
romankrej | 0:f3b355df6f26 | 224 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 225 | |
romankrej | 0:f3b355df6f26 | 226 | if(volts < 0.03 || volts > 0.93) { |
romankrej | 0:f3b355df6f26 | 227 | programMode = WAITING; |
romankrej | 0:f3b355df6f26 | 228 | first = 0; |
romankrej | 0:f3b355df6f26 | 229 | } |
romankrej | 0:f3b355df6f26 | 230 | else { |
romankrej | 0:f3b355df6f26 | 231 | if(programMode == WAITING) |
romankrej | 0:f3b355df6f26 | 232 | programMode = STARTING; |
romankrej | 0:f3b355df6f26 | 233 | } |
romankrej | 0:f3b355df6f26 | 234 | |
romankrej | 0:f3b355df6f26 | 235 | if(programMode == STARTING) |
romankrej | 0:f3b355df6f26 | 236 | first++; |
romankrej | 0:f3b355df6f26 | 237 | |
romankrej | 0:f3b355df6f26 | 238 | if(first > 120) |
romankrej | 0:f3b355df6f26 | 239 | programMode = RUNNING; |
romankrej | 0:f3b355df6f26 | 240 | |
romankrej | 0:f3b355df6f26 | 241 | if(programMode == RUNNING) { |
romankrej | 0:f3b355df6f26 | 242 | phi = 1.1439 * volts - 0.3718; |
romankrej | 0:f3b355df6f26 | 243 | if(first > 120) { |
romankrej | 0:f3b355df6f26 | 244 | phi_old = phi; |
romankrej | 0:f3b355df6f26 | 245 | } |
romankrej | 0:f3b355df6f26 | 246 | |
romankrej | 0:f3b355df6f26 | 247 | |
romankrej | 0:f3b355df6f26 | 248 | omega_temp = (phi - phi_old) * 66.67; |
romankrej | 0:f3b355df6f26 | 249 | if(first > 120) { |
romankrej | 0:f3b355df6f26 | 250 | omega = omega_temp; |
romankrej | 0:f3b355df6f26 | 251 | first = 0; |
romankrej | 0:f3b355df6f26 | 252 | } |
romankrej | 0:f3b355df6f26 | 253 | else { |
romankrej | 0:f3b355df6f26 | 254 | if((omega_temp > omega + 3.0 ) || (omega_temp < omega - 3.0)){ |
romankrej | 0:f3b355df6f26 | 255 | omega_temp = omega; |
romankrej | 0:f3b355df6f26 | 256 | } |
romankrej | 0:f3b355df6f26 | 257 | omega = 0.8 * omega + 0.2 * omega_temp; |
romankrej | 0:f3b355df6f26 | 258 | } |
romankrej | 0:f3b355df6f26 | 259 | } |
romankrej | 0:f3b355df6f26 | 260 | phi_old = phi; |
romankrej | 0:f3b355df6f26 | 261 | } |
romankrej | 0:f3b355df6f26 | 262 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 263 | states.phi1 = phi; |
romankrej | 0:f3b355df6f26 | 264 | states.omega1 = omega; |
romankrej | 0:f3b355df6f26 | 265 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 266 | |
romankrej | 0:f3b355df6f26 | 267 | break; |
romankrej | 0:f3b355df6f26 | 268 | case 0x02 : |
romankrej | 0:f3b355df6f26 | 269 | |
romankrej | 0:f3b355df6f26 | 270 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 271 | states.phi2 = can.getPhi(); |
romankrej | 0:f3b355df6f26 | 272 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 273 | break; |
romankrej | 0:f3b355df6f26 | 274 | case 0x03 : |
romankrej | 0:f3b355df6f26 | 275 | |
romankrej | 0:f3b355df6f26 | 276 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 277 | states.omega2 = can.getOmega(); |
romankrej | 0:f3b355df6f26 | 278 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 279 | break; |
romankrej | 0:f3b355df6f26 | 280 | |
romankrej | 0:f3b355df6f26 | 281 | } |
romankrej | 0:f3b355df6f26 | 282 | }} |
romankrej | 0:f3b355df6f26 | 283 | } |
romankrej | 0:f3b355df6f26 | 284 | } |
romankrej | 0:f3b355df6f26 | 285 | |
romankrej | 0:f3b355df6f26 | 286 | |
romankrej | 0:f3b355df6f26 | 287 | /* |
romankrej | 0:f3b355df6f26 | 288 | * This thread sends SYNC object to servoamplifier |
romankrej | 0:f3b355df6f26 | 289 | */ |
romankrej | 0:f3b355df6f26 | 290 | void syncThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 291 | while(true) { |
romankrej | 0:f3b355df6f26 | 292 | can.sync(); |
romankrej | 0:f3b355df6f26 | 293 | Thread::wait(2); |
romankrej | 0:f3b355df6f26 | 294 | } |
romankrej | 0:f3b355df6f26 | 295 | } |
romankrej | 0:f3b355df6f26 | 296 | |
romankrej | 0:f3b355df6f26 | 297 | |
romankrej | 0:f3b355df6f26 | 298 | /* |
romankrej | 0:f3b355df6f26 | 299 | * This thread print states of system to local file system |
romankrej | 0:f3b355df6f26 | 300 | */ |
romankrej | 0:f3b355df6f26 | 301 | void printThread(void const *args) { |
romankrej | 0:f3b355df6f26 | 302 | while(true) { |
romankrej | 0:f3b355df6f26 | 303 | if(programMode != STOP) { |
romankrej | 0:f3b355df6f26 | 304 | mutex1.lock(); |
romankrej | 0:f3b355df6f26 | 305 | pc.printStates(); |
romankrej | 0:f3b355df6f26 | 306 | mutex1.unlock(); |
romankrej | 0:f3b355df6f26 | 307 | } |
romankrej | 0:f3b355df6f26 | 308 | Thread::wait(15); |
romankrej | 0:f3b355df6f26 | 309 | } |
romankrej | 0:f3b355df6f26 | 310 | } |
romankrej | 0:f3b355df6f26 | 311 |