Revision 1:28d74f044818, committed 2015-04-28

Comitter:

romankrej

Date:

Tue Apr 28 18:48:50 2015 +0000

Parent:

0:f3b355df6f26

Commit message:

DP

Changed in this revision

--- a/control.cpp	Sun Apr 26 13:14:02 2015 +0000
+++ b/control.cpp	Tue Apr 28 18:48:50 2015 +0000
@@ -3,6 +3,7 @@
 
 cControl::cControl() {
     K[0] = 9.6; K[1] = 2.3; K[2] = -0.12; K[3] = -0.2;
+    //K[0] = 10.0; K[1] = 2.4; K[2] = -0.15; K[3] = -0.25;
     q[0] = 24;
     q[1] = -39.999;
     q[2] = 16;

--- a/main.cpp	Sun Apr 26 13:14:02 2015 +0000
+++ b/main.cpp	Tue Apr 28 18:48:50 2015 +0000
@@ -13,28 +13,16 @@
     
     Thread threadUs(usThread,NULL,osPriorityHigh);
     Thread threadLaser(laserThread,NULL,osPriorityHigh);
-    
     if(sensor == ULTRA) {
         threadLaser.terminate();
     }
     if(sensor == LASER)
         threadUs.terminate();
-    
-       
-    Thread threadSync(syncThread,NULL,osPriorityNormal);
-    pc.log("Vytvoreno vlakno synchronizace");
-    
-    Thread threadControl(controlThread,NULL,osPriorityHigh);
-    pc.log("Vytvoreno vlakno pro rizeni");
-    
-    Thread threadCollect(collectThread,NULL,osPriorityHigh);
-    pc.log("Vytvoreno vlakno pro sber dat");
-    
-    Thread threadPrint(printThread,NULL,osPriorityLow);
-    pc.log("Vytvoreno vlakno pro vypis");
-    
-    Thread threadLed(ledThread,NULL,osPriorityHigh);        
-    pc.log("Vytvoreno vlakno pro zobrazeni stavu programu");
+    Thread threadSync(syncThread,NULL,osPriorityAboveNormal);    
+    Thread threadControl(controlThread,NULL,osPriorityHigh);    
+    Thread threadCollect(collectThread,NULL,osPriorityRealtime); 
+    Thread threadPrint(printThread,NULL,osPriorityLow);    
+    Thread threadLed(ledThread,NULL,osPriorityAboveNormal);    
     
     butOff.rise(&stopProg);
     us100.thread = &threadCollect;

--- a/threads.cpp	Sun Apr 26 13:14:02 2015 +0000
+++ b/threads.cpp	Tue Apr 28 18:48:50 2015 +0000
@@ -25,6 +25,16 @@
 cLedSensor baumer(p20);
 cControl control;
 
+/*
+* Beh vlaken
+*/
+DigitalOut vl1(p28);
+DigitalOut vl2(p27);
+DigitalOut vl3(p26);
+DigitalOut vl4(p25);
+DigitalOut vl5(p24);
+DigitalOut vl6(p23);
+
 
 char first;
 states_t states;
@@ -74,13 +84,14 @@
 */
 void ledThread(void const *args) {
     while(true) {
+        vl1 = !vl1;
         switch(programMode) {              
             case WAITING: 
                 ledMode = 1;
                 can.nullResolver();  
                 break;
             case STARTING: 
-                ledMode = 1; 
+                ledMode = 0; 
                 break;
             case RUNNING:
                 ledMode = !ledMode; 
@@ -110,10 +121,8 @@
 */
 void usThread(void const *args) {
     while(true) { 
-        if(programMode != STOP) {
-            can.actual_speed();
-            us100.setTrig();
-        }
+        can.actual_speed();
+        us100.setTrig();
         Thread::wait(15);
     }
 }
@@ -123,11 +132,10 @@
 * This thread requires reading analog value every 3 ms 
 */
 void laserThread(void const *args) {
-    while(true) { 
+    while(true) {
+        vl2 = !vl2; 
         can.actual_speed();
-        if(programMode != STOP) {
-            baumer.read();
-        }
+        baumer.read();
         Thread::wait(3);
     }
 }
@@ -138,12 +146,11 @@
 */
 void controlThread(void const *args) {
     while(true) {
-        if(programMode != STOP) {   
+        vl3 = !vl3;
         mutex1.lock();      
         control.setCurrent();
         can.setCurrent(states.current);            
         mutex1.unlock();
-        }
         Thread::wait(15);
         
     }
@@ -153,13 +160,15 @@
 * This thread thread collect the states of system
 */
 void collectThread(void const *args) {
-    while(true) { 
+    while(true) {
+         
         osEvent ev = Thread::signal_wait(0);
-        if(programMode != STOP) {
         float timeout, volts,phi_temp,phi_old,omega_temp,phi,omega;
         if(ev.status == osEventSignal) {
+            if(programMode != STOP){
             switch(ev.value.signals) {
                 case 0x01 :
+                vl4 = !vl4;
                 if(sensor == ULTRA) {
                     timeout = us100.getPulseWidth();
                     mutex1.lock();
@@ -263,23 +272,22 @@
                     states.phi1 = phi;
                     states.omega1 = omega;
                     mutex1.unlock();
-
                     break;
                 case 0x02 :
-                     
+                    vl4 = !vl4; 
                     mutex1.lock();
                     states.phi2 = can.getPhi();
                     mutex1.unlock();
                     break;
                 case 0x03 :
-                    
+                    vl4 = !vl4;
                     mutex1.lock();
                     states.omega2 = can.getOmega();
                     mutex1.unlock();
                     break;
             
-            }                 
-        }}
+            }}                 
+        }
     }
 }
 
@@ -289,6 +297,7 @@
 */
 void syncThread(void const *args) {
     while(true) {
+        vl5 = !vl5;
         can.sync();
         Thread::wait(2);
     }
@@ -301,6 +310,7 @@
 void printThread(void const *args) {
     while(true) {
         if(programMode != STOP) {
+            vl6 = !vl6;
             mutex1.lock();
             pc.printStates();
             mutex1.unlock();