ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
PiControlThread.cpp
- Committer:
- asobhy
- Date:
- 2018-02-09
- Revision:
- 5:b29220d29022
- Parent:
- 4:417e475239c7
File content as of revision 5:b29220d29022:
#include "mbed.h" #include "ui.h" #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" #include "PiController.h" extern int setpoint; extern bool start_counting; extern Serial bluetooth; uint16_t ID, dTime; int dPosition; int vel; int32_t U; int counter =0 ; int dPosition_values[1000]; 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 PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables 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.001); // 50ms sampling rate } /******************************************************************************* * ******** Periodic Timer Interrupt Thread ******** *******************************************************************************/ void PiControlThread(void const *argument) { // time constant calc setpoint = 140; 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) ; //U = PiController(setpoint,dPosition); // time constnant calc. U = setpoint/28; if (U >= 0) { motorDriver_forward(U); } else if (U < 0) { motorDriver_reverse(U); } // start our calculations if( start_counting == true ) { float time_constant = 0; dPosition_values[counter] = dPosition; counter++; // if we reach our setpoint if (dPosition > 8 ) { // stop counting time start_counting = false; // find the value where dPosition = 316 for(int i=0; i<1000; i++) { if( dPosition_values[i] >= 7 ) { time_constant = counter; //time_constant = time_constant; bluetooth.printf("\ntime constant: %f", time_constant); } } } } } } /******************************************************************************* * 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); }