Revision 0:f3b355df6f26, committed 2015-04-26

Comitter:

romankrej

Date:

Sun Apr 26 13:14:02 2015 +0000

Commit message:

Diplomova prace 2015

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastAnalogIn.lib	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/FastAnalogIn/#afc3b84dbbd6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,207 @@
+#include "can.h"
+#include "threads.h"
+
+CANMessage canmsg;
+
+/*
+ * Constructor
+*/
+cCan::cCan(PinName CANpin1, PinName CANpin2) : Can(CANpin1,CANpin2) {
+    
+    Can.frequency(CAN_FREQ);
+    Can.attach(this,&cCan::CAN_frame_received);
+    cCan::setMode(MOMENT_MODE);
+    cCan::enableServo();
+    cCan::nullResolver();
+}
+
+
+/* 
+ * This method sends synchronization frame
+ */
+void cCan::sync(void) {
+    Can.write(CANMessage(0x80)); 
+}
+
+
+/*
+* This method convert bytes to data
+*/
+int cCan::get_data(char data[], char size) {
+    int ret_data = 0;
+    for(int i = 0; i < 2*size; i++) {
+        ret_data |= data[i] << 8*i; 
+    }
+    return ret_data;
+}
+
+/*
+* This method seds SDO request to servoamplifier
+*/
+int cCan::send_SDO_request(char cmd, int adr, char size,int data) {
+    char data_send[8];
+    data_send[0] = cmd;
+    data_send[1] = 0xFF & adr;;
+    data_send[2] = (0xFF00 & adr) >> 8;
+    data_send[3] = size;
+    data_send[4] = 0xFF & data;
+    data_send[5] = (0xFF00 & data) >> 8;
+    data_send[6] = (0xFF0000 & data)>>16;
+    data_send[7] = (0xFF000000 & data)>>24;
+    if(Can.write(CANMessage(SDO_REQUEST_ID,data_send,8)))
+        return 1;
+    else
+        return 0;
+}
+
+
+/*
+* This method sends PDO with required current
+*/
+int cCan::send_PDO_current(float curr) {
+    char data_send[6];
+    int cur = (int)(-curr*1434.816);
+    data_send[0] = 0xFF & cur;
+    data_send[1] = (0xFF00 & cur) >> 8; 
+    data_send[2] = 0x00;
+    data_send[3] = 0x00;
+    data_send[4] = 0x00;
+    data_send[5] = 0x00;
+    if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6))) {
+        return 1;
+    }
+    else {
+        
+        return 0;
+    }
+}
+
+/*
+* This method sends SDO request of actual speed
+*/
+void cCan::actual_speed() {
+    char data_send[8];
+    data_send[0] = CMD_SDO_READ_REQ;
+    data_send[1] = 0x9A;
+    data_send[2] = 0x01;
+    data_send[3] = 0x2;
+    data_send[4] = 0x0;
+    data_send[5] = 0x0;
+    data_send[6] = 0x0;
+    data_send[7] = 0x0;
+    
+    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
+}
+
+/*
+* This method send SDO request of mode of servo
+*/
+void cCan::get_mode() {
+    char data_send[8];
+    data_send[0] = CMD_SDO_READ_REQ;
+    data_send[1] = 0xEF;
+    data_send[2] = 0x01;
+    data_send[3] = 0x2;
+    data_send[4] = 0x0;
+    data_send[5] = 0x0;
+    data_send[6] = 0x0;
+    data_send[7] = 0x0;
+    
+    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
+}
+
+/*
+* This method is function when CAN frame received
+*/
+void cCan::CAN_frame_received(void) {
+    char curr[2];
+    char pos[4] = {0,0,0,0};
+    Can.read(canmsg);
+    switch(canmsg.id) {
+        
+        case SDO_RESPONSE_ID: 
+            cmd = canmsg.data[0];
+            address[0] = canmsg.data[1];
+            address[1] = canmsg.data[2];
+            data_size = canmsg.data[3];
+            data[0] = canmsg.data[4];
+            data[1] = canmsg.data[5];
+            data[2] = canmsg.data[6];
+            data[3] = canmsg.data[7];
+            
+            if((address[0] == 0x9A) && (address[1] == 0x1)){
+                omega = (((float)get_data(data, 0x02)/286331.153)*-0.10472)/7.7;
+                thread->signal_set(0x03);
+            }
+            
+            if((address[0] == 0xEF) && (address[1] == 0x1))
+                mode = get_data(data, 0x02);
+            break;
+            
+        case PDO_TRANSMIT_ID: 
+
+            pos[0] = canmsg.data[1];
+            pos[1] = canmsg.data[2];
+            pos[2] = canmsg.data[3];
+            pos[3] = canmsg.data[4];
+            curr[0] = canmsg.data[5];
+            curr[1] = canmsg.data[6];
+            
+            current = (float)(short int)get_data(curr,1)*(-0.000697);
+            phi = (float)(get_data(pos,2))*(-0.000012448);
+            thread->signal_set(0x02);
+            break;
+        
+        default: 
+            break;
+    }
+}
+
+/*
+* For enable servo
+*/
+void cCan::enableServo() {
+    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_ENABLE_SERVO);
+}
+
+/*
+* For disable servo
+*/
+void cCan::disableServo() {
+    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_DISABLE_SERVO);
+}
+
+/*
+* For null resolver
+*/
+void cCan::nullResolver() {
+    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_NULL_RES);
+}
+
+/*
+* For set mode of servo
+*/
+void cCan::setMode(int req_mode) {
+    send_SDO_request(CMD_SDO_WRITE_REQ_4,SERVO_MODE,2,req_mode);
+}
+
+/*
+* For setting required current
+*/
+void cCan::setCurrent(float cur) {
+    send_PDO_current(cur);
+}
+
+/*
+*Get angular position of wheel
+*/
+float cCan::getPhi() {
+    return phi;
+}
+
+/*
+* Get angular velocity of wheel 
+*/
+float cCan::getOmega() {
+    return omega;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can.h	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,115 @@
+/*
+* File:         can.h
+* Description:  jojo
+* Version:      1.1
+* Created:      1. 2. 2015
+* Author:       Roman Krejci
+*/
+#ifndef CANPROTOCOL_H
+#define CANPROTOCOL_H
+
+#define ID_SERVO            0x01
+#define SDO_REQUEST_ID      0x600+ID_SERVO
+#define SDO_RESPONSE_ID     0x580+ID_SERVO
+#define PDO_TRANSMIT_ID     0x180+ID_SERVO
+#define PDO_RECEIVE_ID      0x200+ID_SERVO
+#define SYNC_ID             0x80
+  
+#define SERVO_MODE          0x1EF
+#define MASTER_CMD          0x1ED
+
+
+#define CMD_SDO_READ_REQ        0x40
+#define CMD_SDO_READ_RES_2      0x4B
+#define CMD_SDO_READ_RES_4      0x43
+
+#define CMD_SDO_WRITE_REQ_24    0x22
+#define CMD_SDO_WRITE_REQ_2     0x2B         
+#define CMD_SDO_WRITE_REQ_4     0x23
+#define CMD_SDO_WRITE_RES       0x60
+#define CMD_SDO_WRITE_ERR       0x80
+
+
+#define REQ_DISABLE_SERVO       0x01
+#define REQ_ENABLE_SERVO        0x02
+#define REQ_NULL_RES            0x07
+
+
+#define SPEED_MODE              0x200002
+#define MOMENT_MODE             0x200001
+
+#define CAN_FREQ                500000
+
+#include "mbed.h"
+#include "rtos.h"
+
+class cCan 
+{
+    public: 
+        cCan(PinName CANpin1,PinName CANpin2);
+        void CAN_frame_received(void);
+        void sync(void);
+        
+        /* For enable servo */
+        void enableServo(void);
+        
+        /* For disable servo */
+        void disableServo(void);
+        
+        /* For null resolver */
+        void nullResolver(void);
+        
+        /* For set mode of servo */
+        void setMode(int req_mode);
+        
+        /* This method send SDO request of mode of servo */
+        void get_mode(void);
+        
+        /* Get angular position of wheel */
+        float getPhi(void);
+        
+        /* Get angular velocity of wheel */
+        float getOmega(void);
+        
+        
+        /* For setting required current */
+        void setCurrent(float cur);
+        
+        /* This method sends SDO request of actuall speed */
+        void actual_speed(void); 
+        
+        int mode;
+        
+        /* Pointer to collect thread */
+        Thread* thread;
+    
+    private:
+        /* CAN interface */
+        CAN Can;
+        
+        /* */
+        char data[4], address[2], data_size, cmd; 
+        
+        /* angular position of wheel */
+        float phi;
+        
+        /* angular velocity of wheel */
+        float omega;
+        
+        /* current of motor*/
+        float current;
+
+        /* This method convert bytes to data */
+        int get_data(char data[], char size);
+        
+        /* This method seds SDO request to servoamplifier */
+        int send_SDO_request(char cmd, int adr, char size,int data);
+        
+        /* This method sends PDO with required current */
+        int send_PDO_current(float curr);
+    
+
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,55 @@
+#include "control.h"
+#include "threads.h"
+
+cControl::cControl() {
+    K[0] = 9.6; K[1] = 2.3; K[2] = -0.12; K[3] = -0.2;
+    q[0] = 24;
+    q[1] = -39.999;
+    q[2] = 16;
+    e[0] = 0;
+    e[1] = 0;
+    e[2] = 0;
+    u_old = 0;
+}
+
+void cControl::setCurrent() {
+    float temp = 0;
+    if(programMode == RUNNING) {
+        /* STATE FEEDBACK */
+        if(controller == ST_FEEDBACK) {
+            temp = -K[0]*states.phi1 - K[1]*states.omega1 - K[2]*states.phi2 - K[3]*states.omega2;
+            
+            if(temp > 10.0)
+                states.current = 10.0;
+            
+            else {
+                
+                if(temp < -10.0)
+                    states.current = -10.0;
+                
+                else
+                    states.current = temp;
+            }
+        }
+        /* PID CONTROLLER */
+        else {   
+            float temp;
+            e[0] = -states.phi1;
+            temp = u_old  + q[0]*e[0] + q[1]*e[1] + q[2]*e[2];
+            if(temp > 10.0)
+                   states.current = 10.0;
+            else {
+                if(temp < -10.0)
+                    states.current = -10.0;
+                else
+                    states.current = temp;
+            }
+            
+            e[1] = e[0];
+            e[2] = e[1];
+            u_old = states.current;
+        }
+    }
+    else
+        states.current = 0.0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.h	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,23 @@
+#ifndef _CONTROL_H
+#define _CONTROL_H
+
+#include "mbed.h"
+class cControl {
+    public:
+        
+        /* Constructor */
+        cControl();
+        
+        /* This method compute required current and save it to shared resource */        
+        void setCurrent();
+    
+    private: 
+        /* State feedback constants */
+        float K[4];
+        
+        float q[3];
+        float e[3];
+        float u_old;
+
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ledsensor.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,30 @@
+#include "ledsensor.h"
+#include "threads.h"
+
+
+/*-- konstruktor --*/
+cLedSensor::cLedSensor(PinName pinAnalog) : analog(pinAnalog) {
+    voltage = 0;
+    volt_temp = 0;
+    i = 0;
+}
+
+cLedSensor::~cLedSensor() 
+{
+
+}
+
+void cLedSensor::read(void) {
+    volt_temp = analog.read();
+    volt_s[i] = volt_temp;
+    i++;
+    if(i >= 5) {
+        voltage = (volt_s[0] + volt_s[1] + volt_s[2] + volt_s[3] + volt_s[4]) / 5.0;
+        i = 0;
+        thread->signal_set(0x01);
+    }
+}
+
+float cLedSensor::getVoltage(void) {
+    return voltage;  
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ledsensor.h	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,34 @@
+#ifndef _ANGLE_H
+#define _ANGLE_H
+
+#include "mbed.h"
+#include "rtos.h"
+#include "FastAnalogIn.h"
+
+class cLedSensor {
+    public:     
+        /* Constructor */
+        cLedSensor(PinName pinAnalog);
+        
+        /* Desctructor*/
+        ~cLedSensor();
+        
+        /* Get analog value of voltage */
+        float getVoltage();
+        
+        /* This method reads analog value */
+        void read();
+        
+        /* Pointer to collect thread */
+        Thread* thread;
+    
+    private:
+        /*Fast analogIn pin*/
+        FastAnalogIn analog;
+        
+        float voltage;
+        float volt_s[5];
+        float volt_temp;
+        int i;  
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "threads.h"
+#include "ultrasonic.h"
+#include "ledsensor.h"
+#include "can.h"
+#include "control.h"
+#include "print.h"
+
+
+int main() {
+    Thread threadInit(initThread,NULL,osPriorityHigh);
+    
+    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");
+    
+    butOff.rise(&stopProg);
+    us100.thread = &threadCollect;
+    baumer.thread = &threadCollect;
+    can.thread = &threadCollect;
+    
+    Thread::wait(osWaitForever);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#63988a2238f7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7e07b6fb45cf
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/print.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,39 @@
+#include "print.h"
+#include "threads.h"
+
+
+cPrint::cPrint(PinName pinT, PinName pinR): s(pinT,pinR) {
+    s.baud(9600);
+    timer.start();
+    t = 0;
+    LocalFileSystem local("local");
+    cPrint::open();
+};
+
+void cPrint::log (char *message) {
+    s.printf("* * * %s * * *\n",message);
+}
+
+void cPrint::print(float data) {
+    s.printf("%3.3f\n",data);
+    
+}
+
+void cPrint::print(char data) {
+    s.printf("%d\n",data);
+    
+}
+
+void cPrint::printStates() {
+    t = timer.read();
+    fprintf(fp, "%2.4f %2.4f %2.4f %2.4f %2.4f %2.4f \r\n",states.phi1,states.omega1,states.phi2,states.omega2,states.current,t);
+}
+
+int cPrint::close() {
+    timer.stop();
+    return fclose(fp);
+}
+
+void cPrint::open() {
+    fp = fopen("/local/charky.csv", "w");
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/print.h	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,24 @@
+#ifndef _PRINT_H
+#define _PRINT_H
+
+#include "mbed.h"
+#include "rtos.h"
+
+class cPrint {
+    private:
+        Serial s;
+        FILE *fp;
+        Timer timer;
+        float t;
+    public:
+        cPrint(PinName pinT, PinName pinR);
+        void log(char *message);
+        void print(float data);
+        void print(int data);
+        void print(char data);
+        void printStates();
+        void open();
+        int close();
+};
+
+#endif
\ No newline at end of file

--- /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);
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/threads.h	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,98 @@
+#ifndef _COMMON_H
+#define _COMMON_H
+
+
+#include "mbed.h"
+#include "rtos.h"
+#include "can.h"
+#include "ultrasonic.h"
+#include "ledsensor.h"
+#include "control.h"
+#include "print.h"
+
+/* PROGRAM MODES */
+#define WAITING  0x01
+#define STARTING 0x02
+#define RUNNING  0x03
+#define STOP     0x04
+
+
+#define ULTRA   0
+#define LASER   1
+
+
+#define PID   0
+#define ST_FEEDBACK   1
+
+typedef struct states_t {
+    float phi1;
+    float omega1;
+    float phi2;
+    float omega2;
+    float current;  
+} states_t;
+
+
+
+extern InterruptIn butOff;
+
+extern cUltrasonic us100;
+extern cCan can;
+extern cPrint pc;
+extern cLedSensor baumer;
+extern cControl control;
+
+extern float K[4];
+extern char first;
+extern states_t states;
+extern Mutex mutex1;
+extern char  programMode;
+extern char stisknut;
+extern char sensor;
+extern char controller;
+
+
+void stopProg();
+
+
+/* 
+ * Thread for inicialization
+ */
+void initThread(void const *args);
+
+/*
+ * Thread for led diodes
+ */
+void ledThread(void const *args);
+
+/*
+ * Thread for ultrasonic sensor
+ */
+void usThread(void const *args);
+
+/*
+ * Thread for baumer sensor
+ */
+void laserThread(void const *args);
+
+/*
+ * Thread for control
+ */
+void controlThread(void const *args);
+
+/*
+ * Thread for collect data
+ */
+void collectThread(void const *args);
+
+/*
+ * Thread for sending synchronization
+ */
+void syncThread(void const *args);
+
+/*
+ * Thread for print results
+ */
+void printThread(void const *args);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,49 @@
+#include "ultrasonic.h"
+#include "threads.h"
+
+
+/*
+* Constructor 
+*/
+cUltrasonic::cUltrasonic(PinName pinEcho, PinName pinTrig) : echo(pinEcho), trig(pinTrig) {
+    echo.rise(this,&cUltrasonic::riseEdge);
+    echo.fall(this,&cUltrasonic::fallEdge);
+}
+
+
+/* 
+* This method set trigger
+*/
+void cUltrasonic::setTrig(void) {
+        trig.write(0);
+        wait_us(5);
+        trig.write(1); 
+        wait_us(10);
+        trig.write(0);
+}
+
+/* 
+* ISR of rising edge of received pulse 
+* in this thread the timer starts
+*/
+void cUltrasonic::riseEdge(void) {
+    timer.start();  
+}
+
+/*
+* ISR of falling edge of received pulse
+* in this thread is saved width of pulse and signal set to collect thread
+*/
+void cUltrasonic::fallEdge(void) {
+    timer.stop();
+    pulseWidth = timer.read_us();
+    timer.reset();
+    thread->signal_set(0x01);
+}
+
+/*
+* This method returns width  of pulse
+*/
+int cUltrasonic::getPulseWidth(void) {
+    return pulseWidth;  
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.h	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,35 @@
+#ifndef _ULTRASONIC_H
+#define _ULTRASONIC_H
+
+#include "mbed.h"
+#include "rtos.h"
+
+class cUltrasonic {
+    public:     
+        cUltrasonic(PinName pinEcho, PinName pinTrig);
+        
+        /* This method set trigger */
+        void setTrig();
+        
+        /* This method returns width of received pulse*/
+        int getPulseWidth();
+        
+        /* Pointer to thread*/
+        Thread* thread;
+    
+    private:
+        DigitalOut trig;
+        InterruptIn echo;
+        
+        Timer timer;
+        float pulseWidth;
+        
+        /* ISR of rising edge */
+        void riseEdge(void);
+        
+        /* ISR of falling edge*/
+        void fallEdge(void);
+    
+
+};
+#endif
\ No newline at end of file