![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ManualControlFinal by
PiControlThread.cpp@3:4def4ca68910, 2018-02-03 (annotated)
- Committer:
- asobhy
- Date:
- Sat Feb 03 00:48:47 2018 +0000
- Revision:
- 3:4def4ca68910
- Parent:
- 2:ca2a7430739b
- Child:
- 4:417e475239c7
using the control algorithm in the notes
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 | 0:a355e511bc5d | 6 | |
asobhy | 0:a355e511bc5d | 7 | // global speed variable; |
asobhy | 0:a355e511bc5d | 8 | extern int setpoint; |
asobhy | 0:a355e511bc5d | 9 | extern Serial pc; |
asobhy | 0:a355e511bc5d | 10 | extern Mutex setpoint_mutex; |
asobhy | 0:a355e511bc5d | 11 | |
asobhy | 1:3e9684e81312 | 12 | uint16_t ID, dTime; |
asobhy | 1:3e9684e81312 | 13 | int dPosition; |
asobhy | 1:3e9684e81312 | 14 | int vel; |
asobhy | 0:a355e511bc5d | 15 | |
asobhy | 1:3e9684e81312 | 16 | int32_t e, u, xState; |
asobhy | 1:3e9684e81312 | 17 | |
asobhy | 1:3e9684e81312 | 18 | bool saturationFlag; |
asobhy | 1:3e9684e81312 | 19 | |
asobhy | 1:3e9684e81312 | 20 | float Ki; |
asobhy | 1:3e9684e81312 | 21 | float Kp; |
asobhy | 0:a355e511bc5d | 22 | |
asobhy | 0:a355e511bc5d | 23 | void PiControlThread(void const *); |
asobhy | 0:a355e511bc5d | 24 | void PeriodicInterruptISR(void); |
asobhy | 1:3e9684e81312 | 25 | int SaturatingSubtract(int , int ); |
asobhy | 1:3e9684e81312 | 26 | int SaturatingAdd(int , int ); |
asobhy | 1:3e9684e81312 | 27 | int SaturateValue(int , int ); |
asobhy | 1:3e9684e81312 | 28 | |
asobhy | 0:a355e511bc5d | 29 | |
asobhy | 0:a355e511bc5d | 30 | osThreadId PiControlId; |
asobhy | 0:a355e511bc5d | 31 | |
asobhy | 0:a355e511bc5d | 32 | /******************************************************************************/ |
asobhy | 0:a355e511bc5d | 33 | // osPriorityIdle = -3, ///< priority: idle (lowest) |
asobhy | 0:a355e511bc5d | 34 | // osPriorityLow = -2, ///< priority: low |
asobhy | 0:a355e511bc5d | 35 | // osPriorityBelowNormal = -1, ///< priority: below normal |
asobhy | 0:a355e511bc5d | 36 | // osPriorityNormal = 0, ///< priority: normal (default) |
asobhy | 0:a355e511bc5d | 37 | // osPriorityAboveNormal = +1, ///< priority: above normal |
asobhy | 0:a355e511bc5d | 38 | // osPriorityHigh = +2, ///< priority: high |
asobhy | 0:a355e511bc5d | 39 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
asobhy | 0:a355e511bc5d | 40 | /******************************************************************************/ |
asobhy | 0:a355e511bc5d | 41 | |
asobhy | 0:a355e511bc5d | 42 | // Declare PeriodicInterruptThread as a thread/process |
asobhy | 0:a355e511bc5d | 43 | osThreadDef(PiControlThread, osPriorityRealtime, 1024); |
asobhy | 0:a355e511bc5d | 44 | |
asobhy | 0:a355e511bc5d | 45 | Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt |
asobhy | 0:a355e511bc5d | 46 | |
asobhy | 0:a355e511bc5d | 47 | DigitalOut led3(LED3); |
asobhy | 0:a355e511bc5d | 48 | |
asobhy | 0:a355e511bc5d | 49 | |
asobhy | 0:a355e511bc5d | 50 | void PiControlThreadInit() |
asobhy | 0:a355e511bc5d | 51 | { |
asobhy | 0:a355e511bc5d | 52 | DE0_init(); // initialize FPGA |
asobhy | 0:a355e511bc5d | 53 | motorDriver_init(); // initialize motorDriver |
asobhy | 0:a355e511bc5d | 54 | |
asobhy | 0:a355e511bc5d | 55 | PiControlId = osThreadCreate(osThread(PiControlThread), NULL); |
asobhy | 0:a355e511bc5d | 56 | |
asobhy | 0:a355e511bc5d | 57 | // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval |
asobhy | 0:a355e511bc5d | 58 | // in seconds between interrupts, and start interrupt generation: |
asobhy | 1:3e9684e81312 | 59 | PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate |
asobhy | 1:3e9684e81312 | 60 | |
asobhy | 2:ca2a7430739b | 61 | Kp = 2.2; |
asobhy | 1:3e9684e81312 | 62 | Ki = 0.01; |
asobhy | 1:3e9684e81312 | 63 | |
asobhy | 0:a355e511bc5d | 64 | } |
asobhy | 0:a355e511bc5d | 65 | |
asobhy | 0:a355e511bc5d | 66 | |
asobhy | 0:a355e511bc5d | 67 | /******************************************************************************* |
asobhy | 0:a355e511bc5d | 68 | * ******** Periodic Timer Interrupt Thread ******** |
asobhy | 0:a355e511bc5d | 69 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 70 | void PiControlThread(void const *argument) |
asobhy | 1:3e9684e81312 | 71 | { |
asobhy | 3:4def4ca68910 | 72 | |
asobhy | 1:3e9684e81312 | 73 | // initialization |
asobhy | 1:3e9684e81312 | 74 | saturationFlag = false; |
asobhy | 1:3e9684e81312 | 75 | int scale = 40; |
asobhy | 3:4def4ca68910 | 76 | xState = 0; |
asobhy | 3:4def4ca68910 | 77 | |
asobhy | 3:4def4ca68910 | 78 | int32_t xTemp; |
asobhy | 3:4def4ca68910 | 79 | int32_t uProportional; |
asobhy | 3:4def4ca68910 | 80 | int32_t uIntegral; |
asobhy | 3:4def4ca68910 | 81 | int32_t uS; |
asobhy | 1:3e9684e81312 | 82 | |
asobhy | 0:a355e511bc5d | 83 | while (true) |
asobhy | 0:a355e511bc5d | 84 | { |
asobhy | 0:a355e511bc5d | 85 | osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. |
asobhy | 0:a355e511bc5d | 86 | |
asobhy | 0:a355e511bc5d | 87 | // get incremental position and time from QEI |
asobhy | 0:a355e511bc5d | 88 | DE0_read(&ID, &dPosition, &dTime); |
asobhy | 1:3e9684e81312 | 89 | SaturateValue(dPosition, 560); |
asobhy | 1:3e9684e81312 | 90 | |
asobhy | 1:3e9684e81312 | 91 | // maximum velocity at dPostition = 560 is vel = 703 |
asobhy | 1:3e9684e81312 | 92 | vel = (float)((6135.92 * dPosition) / dTime) ; |
asobhy | 0:a355e511bc5d | 93 | |
asobhy | 0:a355e511bc5d | 94 | setpoint_mutex.lock(); |
asobhy | 2:ca2a7430739b | 95 | e = SaturatingSubtract(setpoint, dPosition); // e is the velocity error |
asobhy | 0:a355e511bc5d | 96 | setpoint_mutex.unlock(); |
asobhy | 3:4def4ca68910 | 97 | |
asobhy | 3:4def4ca68910 | 98 | xTemp = SaturatingAdd(xState, e); |
asobhy | 1:3e9684e81312 | 99 | |
asobhy | 1:3e9684e81312 | 100 | // the maximum value that 'u' can get to is 20 |
asobhy | 1:3e9684e81312 | 101 | // the maximum value that dPosition can get to 560 |
asobhy | 1:3e9684e81312 | 102 | // scaling factor is 560/20 = 28 |
asobhy | 3:4def4ca68910 | 103 | uProportional = (float)(Kp*e/scale); |
asobhy | 3:4def4ca68910 | 104 | uIntegral = (float)(Ki*xState/scale); |
asobhy | 1:3e9684e81312 | 105 | |
asobhy | 3:4def4ca68910 | 106 | uS = SaturatingAdd(uProportional, uIntegral); |
asobhy | 1:3e9684e81312 | 107 | |
asobhy | 3:4def4ca68910 | 108 | u = SaturateValue(uS, U_LIMIT); |
asobhy | 3:4def4ca68910 | 109 | if(u==uS) xState=xTemp; |
asobhy | 0:a355e511bc5d | 110 | |
asobhy | 0:a355e511bc5d | 111 | if (u >= 0) |
asobhy | 0:a355e511bc5d | 112 | { |
asobhy | 0:a355e511bc5d | 113 | motorDriver_forward(u); |
asobhy | 0:a355e511bc5d | 114 | } |
asobhy | 0:a355e511bc5d | 115 | else if (u < 0) |
asobhy | 0:a355e511bc5d | 116 | { |
asobhy | 0:a355e511bc5d | 117 | motorDriver_reverse(u); |
asobhy | 1:3e9684e81312 | 118 | } |
asobhy | 3:4def4ca68910 | 119 | |
asobhy | 0:a355e511bc5d | 120 | } |
asobhy | 1:3e9684e81312 | 121 | |
asobhy | 0:a355e511bc5d | 122 | } |
asobhy | 0:a355e511bc5d | 123 | |
asobhy | 0:a355e511bc5d | 124 | /******************************************************************************* |
asobhy | 0:a355e511bc5d | 125 | * the interrupt below occures every 250ms as setup in the main function during |
asobhy | 0:a355e511bc5d | 126 | * initialization |
asobhy | 0:a355e511bc5d | 127 | * ******** Period Timer Interrupt Handler ******** |
asobhy | 0:a355e511bc5d | 128 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 129 | void PeriodicInterruptISR(void) |
asobhy | 0:a355e511bc5d | 130 | { |
asobhy | 0:a355e511bc5d | 131 | // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. |
asobhy | 0:a355e511bc5d | 132 | osSignalSet(PiControlId,0x1); |
asobhy | 0:a355e511bc5d | 133 | } |
asobhy | 1:3e9684e81312 | 134 | |
asobhy | 1:3e9684e81312 | 135 | |
asobhy | 1:3e9684e81312 | 136 | /*****************************************************************************/ |
asobhy | 1:3e9684e81312 | 137 | int SaturatingSubtract(int x, int y) |
asobhy | 1:3e9684e81312 | 138 | { |
asobhy | 1:3e9684e81312 | 139 | int z; |
asobhy | 1:3e9684e81312 | 140 | z = x - y; // 32-bit overflow detection and saturating arithmetic |
asobhy | 1:3e9684e81312 | 141 | if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF; |
asobhy | 1:3e9684e81312 | 142 | else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000; |
asobhy | 1:3e9684e81312 | 143 | return z; |
asobhy | 1:3e9684e81312 | 144 | } |
asobhy | 1:3e9684e81312 | 145 | |
asobhy | 1:3e9684e81312 | 146 | /*****************************************************************************/ |
asobhy | 1:3e9684e81312 | 147 | int SaturatingAdd(int x, int y) |
asobhy | 1:3e9684e81312 | 148 | { |
asobhy | 1:3e9684e81312 | 149 | int z; |
asobhy | 1:3e9684e81312 | 150 | z = x + y; // 32-bit overflow detection and saturating arithmetic |
asobhy | 1:3e9684e81312 | 151 | if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF; |
asobhy | 1:3e9684e81312 | 152 | else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000; |
asobhy | 1:3e9684e81312 | 153 | return z; |
asobhy | 1:3e9684e81312 | 154 | } |
asobhy | 1:3e9684e81312 | 155 | |
asobhy | 1:3e9684e81312 | 156 | /*****************************************************************************/ |
asobhy | 1:3e9684e81312 | 157 | int SaturateValue(int x, int Limit) |
asobhy | 1:3e9684e81312 | 158 | { |
asobhy | 1:3e9684e81312 | 159 | if(x > Limit) return(Limit); // Impose maximum limit on x |
asobhy | 1:3e9684e81312 | 160 | else if(x < -Limit) return(-Limit); |
asobhy | 1:3e9684e81312 | 161 | else return(x); |
asobhy | 1:3e9684e81312 | 162 | } |