File content as of revision 1:28d74f044818:

#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;

/*
* 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;
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) {
        vl1 = !vl1;
        switch(programMode) {              
            case WAITING: 
                ledMode = 1;
                can.nullResolver();  
                break;
            case STARTING: 
                ledMode = 0; 
                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) { 
        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) {
        vl2 = !vl2; 
        can.actual_speed();
        baumer.read();
        Thread::wait(3);
    }
}


/*
* This thread control system
*/
void controlThread(void const *args) {
    while(true) {
        vl3 = !vl3;
        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);
        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();
                    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 :
                    vl4 = !vl4; 
                    mutex1.lock();
                    states.phi2 = can.getPhi();
                    mutex1.unlock();
                    break;
                case 0x03 :
                    vl4 = !vl4;
                    mutex1.lock();
                    states.omega2 = can.getOmega();
                    mutex1.unlock();
                    break;
            
            }}                 
        }
    }
}


/*
* This thread sends SYNC object to servoamplifier
*/
void syncThread(void const *args) {
    while(true) {
        vl5 = !vl5;
        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) {
            vl6 = !vl6;
            mutex1.lock();
            pc.printStates();
            mutex1.unlock();
        }
        Thread::wait(15);
    }
}