PlayBack
Dependencies: TPixy-Interface
Fork of ObjectFollower by
PiControlThread.cpp
- Committer:
- asobhy
- Date:
- 2018-02-01
- Revision:
- 0:a355e511bc5d
- Child:
- 1:3e9684e81312
File content as of revision 0:a355e511bc5d:
#include "mbed.h" #include "ui.h" #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" // global speed variable; extern int setpoint; extern Serial pc; extern Mutex setpoint_mutex; uint16_t ID, dPosition, dTime; int e, u, xState; float Ki, Kp; void PiControlThread(void const *); void PeriodicInterruptISR(void); osThreadId PiControlId; /******************************************************************************/ // osPriorityIdle = -3, ///< priority: idle (lowest) // osPriorityLow = -2, ///< priority: low // osPriorityBelowNormal = -1, ///< priority: below normal // osPriorityNormal = 0, ///< priority: normal (default) // osPriorityAboveNormal = +1, ///< priority: above normal // osPriorityHigh = +2, ///< priority: high // osPriorityRealtime = +3, ///< priority: realtime (highest) /******************************************************************************/ // Declare PeriodicInterruptThread as a thread/process osThreadDef(PiControlThread, osPriorityRealtime, 1024); Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt DigitalOut led3(LED3); void PiControlThreadInit() { DE0_init(); // initialize FPGA motorDriver_init(); // initialize motorDriver PiControlId = osThreadCreate(osThread(PiControlThread), NULL); // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: PeriodicInt.attach(&PeriodicInterruptISR, 0.05); } /******************************************************************************* * ******** Periodic Timer Interrupt Thread ******** *******************************************************************************/ void PiControlThread(void const *argument) { while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled. // get incremental position and time from QEI DE0_read(&ID, &dPosition, &dTime); setpoint_mutex.lock(); e = setpoint-dPosition; // e is the velocity error setpoint_mutex.unlock(); xState = xState + e; // x is the Euler approximation to the integral of e. u = Kp*e + Ki*xState; // u is the control signal if (u >= 0) { motorDriver_forward(u); } else if (u < 0) { motorDriver_reverse(u); } else { pc.printf("\r\nerror!!!"); } } } /******************************************************************************* * the interrupt below occures every 250ms as setup in the main function during * initialization * ******** Period Timer Interrupt Handler ******** *******************************************************************************/ void PeriodicInterruptISR(void) { // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. osSignalSet(PiControlId,0x1); }