Roman Krejci / Mbed 2 deprecated DP_28_4_2015

DP

Dependencies:   FastAnalogIn mbed-rtos mbed

Fork of dipl_prace_v10 by Roman Krejci

threads.cpp@1:28d74f044818, 2015-04-28 (annotated)

Committer:
romankrej
Date:
Tue Apr 28 18:48:50 2015 +0000
Revision:
1:28d74f044818
Parent:
0:f3b355df6f26 
DP

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