Diff: threads.cpp

Revision:

0:f3b355df6f26

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/threads.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,311 @@
+#include "threads.h"
+
+/* 
+* LED diods
+*/
+DigitalOut ledMode(LED1);
+DigitalOut ledSensor(LED2);
+DigitalOut ledController(LED3);
+
+
+/* 
+* Buttons
+*/
+DigitalIn swSensor(p7);
+DigitalIn swController(p8);
+InterruptIn butOff(p5);
+
+
+/*
+* Instances of classes
+*/
+cUltrasonic us100(p12,p13);
+cCan can(p30,p29);
+cPrint pc(USBTX,USBRX);
+cLedSensor baumer(p20);
+cControl control;
+
+
+char first;
+states_t states;
+Mutex mutex1;
+char  programMode;
+char sensor;
+char controller;
+char stisknut;
+
+
+
+
+
+/*
+* ISR of stop button
+*/
+void stopProg() {
+    __disable_irq();
+    stisknut = 1;
+    can.disableServo();
+    programMode = STOP;
+    __enable_irq();
+}  
+
+
+void initThread(void const *args) {
+    programMode = WAITING;
+    sensor = swSensor.read();
+    controller = swController.read();
+    ledSensor = sensor;
+    ledController = controller;
+    if(sensor == ULTRA) {
+        pc.log("Zadan ultrazvukovy snimac");
+    }
+    else {
+        pc.log("Zadan LED snimac");
+    }  
+    if(controller == ST_FEEDBACK)
+        pc.log("Zadana stavova zpetna vazba");
+    else
+        pc.log("Zadan PID regulator");
+}
+
+
+/*
+* This thread indicate current state of program
+*/
+void ledThread(void const *args) {
+    while(true) {
+        switch(programMode) {              
+            case WAITING: 
+                ledMode = 1;
+                can.nullResolver();  
+                break;
+            case STARTING: 
+                ledMode = 1; 
+                break;
+            case RUNNING:
+                ledMode = !ledMode; 
+                break;
+            case STOP:
+                if(stisknut == 1) {
+                    pc.close();
+                    stisknut = 0;
+                }
+                ledMode = 0;
+                ledSensor = 0;
+                ledController = 0;
+        }    
+        if(sensor == ULTRA)
+            ledSensor = !ledSensor;  
+        if(controller == PID)
+            ledController = !ledController;
+        Thread::wait(200);
+    }
+
+}
+
+
+/*
+* Thread for ultrasonic sensor
+* This thread sets trigger every 15 ms
+*/
+void usThread(void const *args) {
+    while(true) { 
+        if(programMode != STOP) {
+            can.actual_speed();
+            us100.setTrig();
+        }
+        Thread::wait(15);
+    }
+}
+
+/*
+* Thread for led sensor
+* This thread requires reading analog value every 3 ms 
+*/
+void laserThread(void const *args) {
+    while(true) { 
+        can.actual_speed();
+        if(programMode != STOP) {
+            baumer.read();
+        }
+        Thread::wait(3);
+    }
+}
+
+
+/*
+* This thread control system
+*/
+void controlThread(void const *args) {
+    while(true) {
+        if(programMode != STOP) {   
+        mutex1.lock();      
+        control.setCurrent();
+        can.setCurrent(states.current);            
+        mutex1.unlock();
+        }
+        Thread::wait(15);
+        
+    }
+}
+
+/*
+* This thread thread collect the states of system
+*/
+void collectThread(void const *args) {
+    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) {
+            switch(ev.value.signals) {
+                case 0x01 :
+                if(sensor == ULTRA) {
+                    timeout = us100.getPulseWidth();
+                    mutex1.lock();
+                    phi_old = states.phi1;
+                    phi = states.phi1;
+                    omega = states.omega1;
+                    mutex1.unlock();
+                    
+                    if(timeout > 2000) {
+                        programMode = WAITING;
+                        first = 0;
+                    }
+                    
+                    else {
+                        if(programMode == WAITING)
+                            programMode = STARTING;
+                    }
+                    
+                    if(programMode == STARTING)
+                        first++;    
+                    
+                    if(first > 120)
+                        programMode = RUNNING;
+                    
+                    if(programMode == RUNNING) {
+                        phi_temp = (float)(0.0005*timeout - 0.4925)+0.02;
+                        if(first > 120) {
+                            phi = phi_temp;
+                            phi_old = phi;
+                        }
+                        
+                        else {
+                            if((phi_temp > phi+0.15)||(phi_temp < phi-0.15)) {
+                                phi_temp = phi;
+                            }
+                        }
+                        
+                        phi = phi_temp;
+        
+                        omega_temp = (phi - phi_old) * 66.67 ;
+                        if(first > 120) {
+                            omega = omega_temp;
+                            first = 0;
+                        }
+                        
+                        else {
+                            if((omega_temp > omega + 3.0 ) || (omega_temp < omega - 3.0)){
+                                omega_temp = omega;
+                            }
+                            omega = 0.8 * omega + 0.2 * omega_temp;
+                        }
+                    }
+                    }
+                    
+                    else {
+                    
+                    volts = baumer.getVoltage();
+                    mutex1.lock();
+                    phi_old = states.phi1;
+                    phi = states.phi1;
+                    omega = states.omega1;
+                    mutex1.unlock();
+                    
+                    if(volts < 0.03 || volts > 0.93) {
+                        programMode = WAITING;
+                        first = 0;
+                    }
+                    else {
+                        if(programMode == WAITING)
+                            programMode = STARTING;
+                    }
+                    
+                    if(programMode == STARTING)
+                        first++;    
+                    
+                    if(first > 120)
+                        programMode = RUNNING;
+                    
+                    if(programMode == RUNNING) {
+                        phi = 1.1439 * volts - 0.3718; 
+                        if(first > 120) {
+                            phi_old = phi;
+                        }
+
+        
+                        omega_temp = (phi - phi_old) * 66.67;
+                        if(first > 120) {
+                            omega = omega_temp;
+                            first = 0;
+                        }
+                        else {
+                            if((omega_temp > omega + 3.0 ) || (omega_temp < omega - 3.0)){
+                                omega_temp = omega;
+                            }
+                            omega = 0.8 * omega + 0.2 * omega_temp;
+                        }
+                    }
+                    phi_old = phi;
+                    }
+                    mutex1.lock();
+                    states.phi1 = phi;
+                    states.omega1 = omega;
+                    mutex1.unlock();
+
+                    break;
+                case 0x02 :
+                     
+                    mutex1.lock();
+                    states.phi2 = can.getPhi();
+                    mutex1.unlock();
+                    break;
+                case 0x03 :
+                    
+                    mutex1.lock();
+                    states.omega2 = can.getOmega();
+                    mutex1.unlock();
+                    break;
+            
+            }                 
+        }}
+    }
+}
+
+
+/*
+* This thread sends SYNC object to servoamplifier
+*/
+void syncThread(void const *args) {
+    while(true) {
+        can.sync();
+        Thread::wait(2);
+    }
+}
+
+
+/*
+* This thread print states of system to local file system
+*/
+void printThread(void const *args) {
+    while(true) {
+        if(programMode != STOP) {
+            mutex1.lock();
+            pc.printStates();
+            mutex1.unlock();
+        }
+        Thread::wait(15);
+    }
+}
+