DP
Dependencies: FastAnalogIn mbed-rtos mbed
threads.cpp
- Committer:
- romankrej
- Date:
- 2015-04-26
- Revision:
- 0:f3b355df6f26
File content as of revision 0:f3b355df6f26:
#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); } }