Roman Krejci / Mbed 2 deprecated dipl_prace_v10

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?

UserRevisionLine numberNew 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