threads.cpp

00001 #include "threads.h"
00002 
00003 /* 
00004 * LED diods
00005 */
00006 DigitalOut ledMode(LED1);
00007 DigitalOut ledSensor(LED2);
00008 DigitalOut ledController(LED3);
00009 
00010 
00011 /* 
00012 * Buttons
00013 */
00014 DigitalIn swSensor(p7);
00015 DigitalIn swController(p8);
00016 InterruptIn butOff(p5);
00017 
00018 
00019 /*
00020 * Instances of classes
00021 */
00022 cUltrasonic us100(p12,p13);
00023 cCan can(p30,p29);
00024 cPrint pc(USBTX,USBRX);
00025 cLedSensor baumer(p20);
00026 cControl control;
00027 
00028 /*
00029 * Beh vlaken
00030 */
00031 DigitalOut vl1(p28);
00032 DigitalOut vl2(p27);
00033 DigitalOut vl3(p26);
00034 DigitalOut vl4(p25);
00035 DigitalOut vl5(p24);
00036 DigitalOut vl6(p23);
00037 
00038 
00039 char first;
00040 states_t states;
00041 Mutex mutex1;
00042 char  programMode;
00043 char sensor;
00044 char controller;
00045 char stisknut;
00046 
00047 
00048 
00049 
00050 
00051 /*
00052 * ISR of stop button
00053 */
00054 void stopProg() {
00055     __disable_irq();
00056     stisknut = 1;
00057     can.disableServo();
00058     programMode = STOP;
00059     __enable_irq();
00060 }  
00061 
00062 
00063 void initThread(void const *args) {
00064     programMode = WAITING;
00065     sensor = swSensor.read();
00066     controller = swController.read();
00067     ledSensor = sensor;
00068     ledController = controller;
00069     if(sensor == ULTRA) {
00070         pc.log("Zadan ultrazvukovy snimac");
00071     }
00072     else {
00073         pc.log("Zadan LED snimac");
00074     }  
00075     if(controller == ST_FEEDBACK)
00076         pc.log("Zadana stavova zpetna vazba");
00077     else
00078         pc.log("Zadan PID regulator");
00079 }
00080 
00081 
00082 /*
00083 * This thread indicate current state of program
00084 */
00085 void ledThread(void const *args) {
00086     while(true) {
00087         vl1 = !vl1;
00088         switch(programMode) {              
00089             case WAITING: 
00090                 ledMode = 1;
00091                 can.nullResolver();  
00092                 break;
00093             case STARTING: 
00094                 ledMode = 0; 
00095                 break;
00096             case RUNNING:
00097                 ledMode = !ledMode; 
00098                 break;
00099             case STOP:
00100                 if(stisknut == 1) {
00101                     pc.close();
00102                     stisknut = 0;
00103                 }
00104                 ledMode = 0;
00105                 ledSensor = 0;
00106                 ledController = 0;
00107         }    
00108         if(sensor == ULTRA)
00109             ledSensor = !ledSensor;  
00110         if(controller == PID)
00111             ledController = !ledController;
00112         Thread::wait(200);
00113     }
00114 
00115 }
00116 
00117 
00118 /*
00119 * Thread for ultrasonic sensor
00120 * This thread sets trigger every 15 ms
00121 */
00122 void usThread(void const *args) {
00123     while(true) { 
00124         can.actual_speed();
00125         us100.setTrig();
00126         Thread::wait(15);
00127     }
00128 }
00129 
00130 /*
00131 * Thread for led sensor
00132 * This thread requires reading analog value every 3 ms 
00133 */
00134 void laserThread(void const *args) {
00135     while(true) {
00136         vl2 = !vl2; 
00137         can.actual_speed();
00138         baumer.read();
00139         Thread::wait(3);
00140     }
00141 }
00142 
00143 
00144 /*
00145 * This thread control system
00146 */
00147 void controlThread(void const *args) {
00148     while(true) {
00149         vl3 = !vl3;
00150         mutex1.lock();      
00151         control.setCurrent();
00152         can.setCurrent(states.current);            
00153         mutex1.unlock();
00154         Thread::wait(15);
00155         
00156     }
00157 }
00158 
00159 /*
00160 * This thread thread collect the states of system
00161 */
00162 void collectThread(void const *args) {
00163     while(true) {
00164          
00165         osEvent ev = Thread::signal_wait(0);
00166         float timeout, volts,phi_temp,phi_old,omega_temp,phi,omega;
00167         if(ev.status == osEventSignal) {
00168             if(programMode != STOP){
00169             switch(ev.value.signals) {
00170                 case 0x01 :
00171                 vl4 = !vl4;
00172                 if(sensor == ULTRA) {
00173                     timeout = us100.getPulseWidth();
00174                     mutex1.lock();
00175                     phi_old = states.phi1;
00176                     phi = states.phi1;
00177                     omega = states.omega1;
00178                     mutex1.unlock();
00179                     
00180                     if(timeout > 2000) {
00181                         programMode = WAITING;
00182                         first = 0;
00183                     }
00184                     
00185                     else {
00186                         if(programMode == WAITING)
00187                             programMode = STARTING;
00188                     }
00189                     
00190                     if(programMode == STARTING)
00191                         first++;    
00192                     
00193                     if(first > 120)
00194                         programMode = RUNNING;
00195                     
00196                     if(programMode == RUNNING) {
00197                         phi_temp = (float)(0.0005*timeout - 0.4925)+0.02;
00198                         if(first > 120) {
00199                             phi = phi_temp;
00200                             phi_old = phi;
00201                         }
00202                         
00203                         else {
00204                             if((phi_temp > phi+0.15)||(phi_temp < phi-0.15)) {
00205                                 phi_temp = phi;
00206                             }
00207                         }
00208                         
00209                         phi = phi_temp;
00210         
00211                         omega_temp = (phi - phi_old) * 66.67 ;
00212                         if(first > 120) {
00213                             omega = omega_temp;
00214                             first = 0;
00215                         }
00216                         
00217                         else {
00218                             if((omega_temp > omega + 3.0 ) || (omega_temp < omega - 3.0)){
00219                                 omega_temp = omega;
00220                             }
00221                             omega = 0.8 * omega + 0.2 * omega_temp;
00222                         }
00223                     }
00224                     }
00225                     
00226                     else {
00227                     
00228                     volts = baumer.getVoltage();
00229                     mutex1.lock();
00230                     phi_old = states.phi1;
00231                     phi = states.phi1;
00232                     omega = states.omega1;
00233                     mutex1.unlock();
00234                     
00235                     if(volts < 0.03 || volts > 0.93) {
00236                         programMode = WAITING;
00237                         first = 0;
00238                     }
00239                     else {
00240                         if(programMode == WAITING)
00241                             programMode = STARTING;
00242                     }
00243                     
00244                     if(programMode == STARTING)
00245                         first++;    
00246                     
00247                     if(first > 120)
00248                         programMode = RUNNING;
00249                     
00250                     if(programMode == RUNNING) {
00251                         phi = 1.1439 * volts - 0.3718; 
00252                         if(first > 120) {
00253                             phi_old = phi;
00254                         }
00255 
00256         
00257                         omega_temp = (phi - phi_old) * 66.67;
00258                         if(first > 120) {
00259                             omega = omega_temp;
00260                             first = 0;
00261                         }
00262                         else {
00263                             if((omega_temp > omega + 3.0 ) || (omega_temp < omega - 3.0)){
00264                                 omega_temp = omega;
00265                             }
00266                             omega = 0.8 * omega + 0.2 * omega_temp;
00267                         }
00268                     }
00269                     phi_old = phi;
00270                     }
00271                     mutex1.lock();
00272                     states.phi1 = phi;
00273                     states.omega1 = omega;
00274                     mutex1.unlock();
00275                     break;
00276                 case 0x02 :
00277                     vl4 = !vl4; 
00278                     mutex1.lock();
00279                     states.phi2 = can.getPhi();
00280                     mutex1.unlock();
00281                     break;
00282                 case 0x03 :
00283                     vl4 = !vl4;
00284                     mutex1.lock();
00285                     states.omega2 = can.getOmega();
00286                     mutex1.unlock();
00287                     break;
00288             
00289             }}                 
00290         }
00291     }
00292 }
00293 
00294 
00295 /*
00296 * This thread sends SYNC object to servoamplifier
00297 */
00298 void syncThread(void const *args) {
00299     while(true) {
00300         vl5 = !vl5;
00301         can.sync();
00302         Thread::wait(2);
00303     }
00304 }
00305 
00306 
00307 /*
00308 * This thread print states of system to local file system
00309 */
00310 void printThread(void const *args) {
00311     while(true) {
00312         if(programMode != STOP) {
00313             vl6 = !vl6;
00314             mutex1.lock();
00315             pc.printStates();
00316             mutex1.unlock();
00317         }
00318         Thread::wait(15);
00319     }
00320 }
00321