PlayBack
Dependencies: TPixy-Interface
Fork of ObjectFollower by
PiControlThread.cpp
- Committer:
- asobhy
- Date:
- 2018-02-03
- Revision:
- 3:4def4ca68910
- Parent:
- 2:ca2a7430739b
- Child:
- 4:417e475239c7
File content as of revision 3:4def4ca68910:
#include "mbed.h" #include "ui.h" #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" // global speed variable; extern int setpoint; extern Serial pc; extern Mutex setpoint_mutex; uint16_t ID, dTime; int dPosition; int vel; int32_t e, u, xState; bool saturationFlag; float Ki; float Kp; void PiControlThread(void const *); void PeriodicInterruptISR(void); int SaturatingSubtract(int , int ); int SaturatingAdd(int , int ); int SaturateValue(int , int ); 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); // 50ms sampling rate Kp = 2.2; Ki = 0.01; } /******************************************************************************* * ******** Periodic Timer Interrupt Thread ******** *******************************************************************************/ void PiControlThread(void const *argument) { // initialization saturationFlag = false; int scale = 40; xState = 0; int32_t xTemp; int32_t uProportional; int32_t uIntegral; int32_t uS; while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. // get incremental position and time from QEI DE0_read(&ID, &dPosition, &dTime); SaturateValue(dPosition, 560); // maximum velocity at dPostition = 560 is vel = 703 vel = (float)((6135.92 * dPosition) / dTime) ; setpoint_mutex.lock(); e = SaturatingSubtract(setpoint, dPosition); // e is the velocity error setpoint_mutex.unlock(); xTemp = SaturatingAdd(xState, e); // the maximum value that 'u' can get to is 20 // the maximum value that dPosition can get to 560 // scaling factor is 560/20 = 28 uProportional = (float)(Kp*e/scale); uIntegral = (float)(Ki*xState/scale); uS = SaturatingAdd(uProportional, uIntegral); u = SaturateValue(uS, U_LIMIT); if(u==uS) xState=xTemp; if (u >= 0) { motorDriver_forward(u); } else if (u < 0) { motorDriver_reverse(u); } } } /******************************************************************************* * 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); } /*****************************************************************************/ int SaturatingSubtract(int x, int y) { int z; z = x - y; // 32-bit overflow detection and saturating arithmetic if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF; else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000; return z; } /*****************************************************************************/ int SaturatingAdd(int x, int y) { int z; z = x + y; // 32-bit overflow detection and saturating arithmetic if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF; else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000; return z; } /*****************************************************************************/ int SaturateValue(int x, int Limit) { if(x > Limit) return(Limit); // Impose maximum limit on x else if(x < -Limit) return(-Limit); else return(x); }