![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ManualControlFinal by
PiControlThread.cpp@6:e7ce340fe91e, 2018-02-10 (annotated)
- Committer:
- asobhy
- Date:
- Sat Feb 10 19:35:21 2018 +0000
- Revision:
- 6:e7ce340fe91e
- Parent:
- 4:417e475239c7
- Child:
- 7:73fd05fe556a
02/10/2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
asobhy | 0:a355e511bc5d | 1 | #include "mbed.h" |
asobhy | 0:a355e511bc5d | 2 | #include "ui.h" |
asobhy | 0:a355e511bc5d | 3 | #include "Drivers/motor_driver.h" |
asobhy | 0:a355e511bc5d | 4 | #include "Drivers/DE0_driver.h" |
asobhy | 1:3e9684e81312 | 5 | #include "PiControlThread.h" |
asobhy | 4:417e475239c7 | 6 | #include "PiController.h" |
asobhy | 0:a355e511bc5d | 7 | |
asobhy | 0:a355e511bc5d | 8 | extern int setpoint; |
asobhy | 0:a355e511bc5d | 9 | |
asobhy | 1:3e9684e81312 | 10 | uint16_t ID, dTime; |
asobhy | 1:3e9684e81312 | 11 | int dPosition; |
asobhy | 1:3e9684e81312 | 12 | int vel; |
asobhy | 4:417e475239c7 | 13 | int32_t U; |
asobhy | 0:a355e511bc5d | 14 | |
asobhy | 0:a355e511bc5d | 15 | void PiControlThread(void const *); |
asobhy | 0:a355e511bc5d | 16 | void PeriodicInterruptISR(void); |
asobhy | 0:a355e511bc5d | 17 | |
asobhy | 0:a355e511bc5d | 18 | osThreadId PiControlId; |
asobhy | 0:a355e511bc5d | 19 | |
asobhy | 0:a355e511bc5d | 20 | /******************************************************************************/ |
asobhy | 0:a355e511bc5d | 21 | // osPriorityIdle = -3, ///< priority: idle (lowest) |
asobhy | 0:a355e511bc5d | 22 | // osPriorityLow = -2, ///< priority: low |
asobhy | 0:a355e511bc5d | 23 | // osPriorityBelowNormal = -1, ///< priority: below normal |
asobhy | 0:a355e511bc5d | 24 | // osPriorityNormal = 0, ///< priority: normal (default) |
asobhy | 0:a355e511bc5d | 25 | // osPriorityAboveNormal = +1, ///< priority: above normal |
asobhy | 0:a355e511bc5d | 26 | // osPriorityHigh = +2, ///< priority: high |
asobhy | 0:a355e511bc5d | 27 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
asobhy | 0:a355e511bc5d | 28 | /******************************************************************************/ |
asobhy | 0:a355e511bc5d | 29 | |
asobhy | 0:a355e511bc5d | 30 | // Declare PeriodicInterruptThread as a thread/process |
asobhy | 0:a355e511bc5d | 31 | osThreadDef(PiControlThread, osPriorityRealtime, 1024); |
asobhy | 0:a355e511bc5d | 32 | |
asobhy | 0:a355e511bc5d | 33 | Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt |
asobhy | 0:a355e511bc5d | 34 | |
asobhy | 0:a355e511bc5d | 35 | DigitalOut led3(LED3); |
asobhy | 0:a355e511bc5d | 36 | |
asobhy | 0:a355e511bc5d | 37 | |
asobhy | 0:a355e511bc5d | 38 | void PiControlThreadInit() |
asobhy | 0:a355e511bc5d | 39 | { |
asobhy | 4:417e475239c7 | 40 | DE0_init(); // initialize FPGA |
asobhy | 4:417e475239c7 | 41 | motorDriver_init(); // initialize motorDriver |
asobhy | 6:e7ce340fe91e | 42 | // Kp,Ki |
asobhy | 6:e7ce340fe91e | 43 | PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables |
asobhy | 0:a355e511bc5d | 44 | |
asobhy | 0:a355e511bc5d | 45 | PiControlId = osThreadCreate(osThread(PiControlThread), NULL); |
asobhy | 0:a355e511bc5d | 46 | |
asobhy | 0:a355e511bc5d | 47 | // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval |
asobhy | 0:a355e511bc5d | 48 | // in seconds between interrupts, and start interrupt generation: |
asobhy | 1:3e9684e81312 | 49 | PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate |
asobhy | 4:417e475239c7 | 50 | |
asobhy | 0:a355e511bc5d | 51 | } |
asobhy | 0:a355e511bc5d | 52 | |
asobhy | 0:a355e511bc5d | 53 | |
asobhy | 0:a355e511bc5d | 54 | /******************************************************************************* |
asobhy | 0:a355e511bc5d | 55 | * ******** Periodic Timer Interrupt Thread ******** |
asobhy | 0:a355e511bc5d | 56 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 57 | void PiControlThread(void const *argument) |
asobhy | 1:3e9684e81312 | 58 | { |
asobhy | 3:4def4ca68910 | 59 | |
asobhy | 0:a355e511bc5d | 60 | while (true) |
asobhy | 0:a355e511bc5d | 61 | { |
asobhy | 0:a355e511bc5d | 62 | osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. |
asobhy | 0:a355e511bc5d | 63 | |
asobhy | 0:a355e511bc5d | 64 | // get incremental position and time from QEI |
asobhy | 0:a355e511bc5d | 65 | DE0_read(&ID, &dPosition, &dTime); |
asobhy | 1:3e9684e81312 | 66 | SaturateValue(dPosition, 560); |
asobhy | 1:3e9684e81312 | 67 | |
asobhy | 1:3e9684e81312 | 68 | // maximum velocity at dPostition = 560 is vel = 703 |
asobhy | 1:3e9684e81312 | 69 | vel = (float)((6135.92 * dPosition) / dTime) ; |
asobhy | 0:a355e511bc5d | 70 | |
asobhy | 4:417e475239c7 | 71 | U = PiController(setpoint,dPosition); |
asobhy | 1:3e9684e81312 | 72 | |
asobhy | 4:417e475239c7 | 73 | if (U >= 0) |
asobhy | 0:a355e511bc5d | 74 | { |
asobhy | 4:417e475239c7 | 75 | motorDriver_forward(U); |
asobhy | 0:a355e511bc5d | 76 | } |
asobhy | 4:417e475239c7 | 77 | else if (U < 0) |
asobhy | 0:a355e511bc5d | 78 | { |
asobhy | 4:417e475239c7 | 79 | motorDriver_reverse(U); |
asobhy | 1:3e9684e81312 | 80 | } |
asobhy | 3:4def4ca68910 | 81 | |
asobhy | 0:a355e511bc5d | 82 | } |
asobhy | 1:3e9684e81312 | 83 | |
asobhy | 0:a355e511bc5d | 84 | } |
asobhy | 0:a355e511bc5d | 85 | |
asobhy | 0:a355e511bc5d | 86 | /******************************************************************************* |
asobhy | 0:a355e511bc5d | 87 | * the interrupt below occures every 250ms as setup in the main function during |
asobhy | 0:a355e511bc5d | 88 | * initialization |
asobhy | 0:a355e511bc5d | 89 | * ******** Period Timer Interrupt Handler ******** |
asobhy | 0:a355e511bc5d | 90 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 91 | void PeriodicInterruptISR(void) |
asobhy | 0:a355e511bc5d | 92 | { |
asobhy | 0:a355e511bc5d | 93 | // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. |
asobhy | 0:a355e511bc5d | 94 | osSignalSet(PiControlId,0x1); |
asobhy | 0:a355e511bc5d | 95 | } |
asobhy | 1:3e9684e81312 | 96 |