deelPID, aanpasbare gains via pods
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed
main.cpp@0:2aa29a0824df, 2018-10-30 (annotated)
- Committer:
- laurencebr
- Date:
- Tue Oct 30 14:17:16 2018 +0000
- Revision:
- 0:2aa29a0824df
pid controller;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
laurencebr | 0:2aa29a0824df | 1 | #include "mbed.h" |
laurencebr | 0:2aa29a0824df | 2 | #include "FastPWM.h" |
laurencebr | 0:2aa29a0824df | 3 | #include "MODSERIAL.h" |
laurencebr | 0:2aa29a0824df | 4 | #include "QEI.h" |
laurencebr | 0:2aa29a0824df | 5 | #include "HIDScope.h" |
laurencebr | 0:2aa29a0824df | 6 | |
laurencebr | 0:2aa29a0824df | 7 | DigitalOut gpo(D0); |
laurencebr | 0:2aa29a0824df | 8 | DigitalOut led(LED_RED); |
laurencebr | 0:2aa29a0824df | 9 | AnalogIn pot1(A0); |
laurencebr | 0:2aa29a0824df | 10 | AnalogIn pot2(A1); |
laurencebr | 0:2aa29a0824df | 11 | AnalogIn pot3(A2); |
laurencebr | 0:2aa29a0824df | 12 | QEI encoder1(D10, D11, NC, 32); |
laurencebr | 0:2aa29a0824df | 13 | QEI encoder2(D12, D13, NC, 32); |
laurencebr | 0:2aa29a0824df | 14 | FastPWM Motor1PWM(D6); |
laurencebr | 0:2aa29a0824df | 15 | DigitalOut Motor1Direction(D7); |
laurencebr | 0:2aa29a0824df | 16 | FastPWM Motor2PWM(D5); //!!!!! Juiste poorten aangeven |
laurencebr | 0:2aa29a0824df | 17 | DigitalOut Motor2Direction(D4); //!!!!! Juiste poort aangeven |
laurencebr | 0:2aa29a0824df | 18 | |
laurencebr | 0:2aa29a0824df | 19 | MODSERIAL pc(USBTX, USBRX); |
laurencebr | 0:2aa29a0824df | 20 | |
laurencebr | 0:2aa29a0824df | 21 | //HIDSCOPE |
laurencebr | 0:2aa29a0824df | 22 | HIDScope scope(4); |
laurencebr | 0:2aa29a0824df | 23 | Ticker scopeTimer; |
laurencebr | 0:2aa29a0824df | 24 | |
laurencebr | 0:2aa29a0824df | 25 | |
laurencebr | 0:2aa29a0824df | 26 | ///Variables |
laurencebr | 0:2aa29a0824df | 27 | |
laurencebr | 0:2aa29a0824df | 28 | double q1Error; |
laurencebr | 0:2aa29a0824df | 29 | double q2Error; |
laurencebr | 0:2aa29a0824df | 30 | double PrevErrorq1[100]; |
laurencebr | 0:2aa29a0824df | 31 | double PrevErrorq2[100]; |
laurencebr | 0:2aa29a0824df | 32 | int n = 100; // Zelfde waarde als PrevErrorarray |
laurencebr | 0:2aa29a0824df | 33 | double q1Ref = 1.0; //VOOR TESTEN |
laurencebr | 0:2aa29a0824df | 34 | double q2Ref; |
laurencebr | 0:2aa29a0824df | 35 | //Double xPos; |
laurencebr | 0:2aa29a0824df | 36 | //Double yPos; |
laurencebr | 0:2aa29a0824df | 37 | double xRef; |
laurencebr | 0:2aa29a0824df | 38 | double yRef; |
laurencebr | 0:2aa29a0824df | 39 | double q1Pos; |
laurencebr | 0:2aa29a0824df | 40 | double q2Pos; |
laurencebr | 0:2aa29a0824df | 41 | double PIDerrorq1; |
laurencebr | 0:2aa29a0824df | 42 | double PIDerrorq2; |
laurencebr | 0:2aa29a0824df | 43 | double IntegralErrorq1 = 0.0; |
laurencebr | 0:2aa29a0824df | 44 | double IntegralErrorq2 = 0.0; |
laurencebr | 0:2aa29a0824df | 45 | double DerivativeErrorq1 = 0.0; |
laurencebr | 0:2aa29a0824df | 46 | double DerivativeErrorq2 = 0.0; |
laurencebr | 0:2aa29a0824df | 47 | double GainP1 = 2.0; |
laurencebr | 0:2aa29a0824df | 48 | double GainI1 = 1.2; |
laurencebr | 0:2aa29a0824df | 49 | double GainD1 = 0.0; |
laurencebr | 0:2aa29a0824df | 50 | double GainP2 = 2.0; |
laurencebr | 0:2aa29a0824df | 51 | double GainI2 = 2.0; |
laurencebr | 0:2aa29a0824df | 52 | double GainD2 = 2.0; |
laurencebr | 0:2aa29a0824df | 53 | double TickerPeriod = 1.0/400.0; |
laurencebr | 0:2aa29a0824df | 54 | |
laurencebr | 0:2aa29a0824df | 55 | Ticker Kikker; |
laurencebr | 0:2aa29a0824df | 56 | int count = 0; |
laurencebr | 0:2aa29a0824df | 57 | int countstep = 0; |
laurencebr | 0:2aa29a0824df | 58 | |
laurencebr | 0:2aa29a0824df | 59 | |
laurencebr | 0:2aa29a0824df | 60 | |
laurencebr | 0:2aa29a0824df | 61 | void UpdateRefPosition() |
laurencebr | 0:2aa29a0824df | 62 | { |
laurencebr | 0:2aa29a0824df | 63 | // Update, (based on EMG or pots) the reference position xRef, yRef |
laurencebr | 0:2aa29a0824df | 64 | //q1Ref = pot1.read()*2*3.1416; |
laurencebr | 0:2aa29a0824df | 65 | //q2Ref = pot2.read()*2*3.1416; |
laurencebr | 0:2aa29a0824df | 66 | } |
laurencebr | 0:2aa29a0824df | 67 | |
laurencebr | 0:2aa29a0824df | 68 | |
laurencebr | 0:2aa29a0824df | 69 | void InverseKinematics() |
laurencebr | 0:2aa29a0824df | 70 | { |
laurencebr | 0:2aa29a0824df | 71 | // Convert, using the inverse kinematics relations, xRef and yRef to q1Ref and q2Ref |
laurencebr | 0:2aa29a0824df | 72 | // (So, update the values of q1Ref and q2Ref) |
laurencebr | 0:2aa29a0824df | 73 | } |
laurencebr | 0:2aa29a0824df | 74 | |
laurencebr | 0:2aa29a0824df | 75 | |
laurencebr | 0:2aa29a0824df | 76 | void ReadCurrentPosition() //Update the coordinates of the end-effector q1Pos, q2Pos |
laurencebr | 0:2aa29a0824df | 77 | { |
laurencebr | 0:2aa29a0824df | 78 | q1Pos = encoder1.getPulses()/32/131.25*2*3.1416; //Position motor 1 in rad |
laurencebr | 0:2aa29a0824df | 79 | q2Pos = encoder2.getPulses()/32/131.25*2*3.1416; //Position motor 2 in rad |
laurencebr | 0:2aa29a0824df | 80 | } |
laurencebr | 0:2aa29a0824df | 81 | |
laurencebr | 0:2aa29a0824df | 82 | void UpdateError() //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos |
laurencebr | 0:2aa29a0824df | 83 | { |
laurencebr | 0:2aa29a0824df | 84 | q1Error = q1Ref - q1Pos; |
laurencebr | 0:2aa29a0824df | 85 | q2Error = q2Ref - q2Pos; |
laurencebr | 0:2aa29a0824df | 86 | |
laurencebr | 0:2aa29a0824df | 87 | //Update PrevErrorq1 and PrevErrorq2 |
laurencebr | 0:2aa29a0824df | 88 | |
laurencebr | 0:2aa29a0824df | 89 | for (int i = 0; i <= n-2 ; i++) |
laurencebr | 0:2aa29a0824df | 90 | { |
laurencebr | 0:2aa29a0824df | 91 | PrevErrorq1[i] = PrevErrorq1[i+1]; |
laurencebr | 0:2aa29a0824df | 92 | PrevErrorq2[i] = PrevErrorq2[i+1]; |
laurencebr | 0:2aa29a0824df | 93 | } |
laurencebr | 0:2aa29a0824df | 94 | |
laurencebr | 0:2aa29a0824df | 95 | PrevErrorq1[n-1] = q1Error; |
laurencebr | 0:2aa29a0824df | 96 | PrevErrorq2[n-1] = q2Error; |
laurencebr | 0:2aa29a0824df | 97 | } |
laurencebr | 0:2aa29a0824df | 98 | |
laurencebr | 0:2aa29a0824df | 99 | void PIDControl(){ |
laurencebr | 0:2aa29a0824df | 100 | // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error |
laurencebr | 0:2aa29a0824df | 101 | |
laurencebr | 0:2aa29a0824df | 102 | |
laurencebr | 0:2aa29a0824df | 103 | // PID control motor 1 |
laurencebr | 0:2aa29a0824df | 104 | //P-Control |
laurencebr | 0:2aa29a0824df | 105 | double P_error1 = GainP1 * q1Error; |
laurencebr | 0:2aa29a0824df | 106 | |
laurencebr | 0:2aa29a0824df | 107 | //I-Control |
laurencebr | 0:2aa29a0824df | 108 | IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod; |
laurencebr | 0:2aa29a0824df | 109 | double I_error1 = GainI1 * IntegralErrorq1; |
laurencebr | 0:2aa29a0824df | 110 | |
laurencebr | 0:2aa29a0824df | 111 | //D-Control |
laurencebr | 0:2aa29a0824df | 112 | //First smoothen the error signal |
laurencebr | 0:2aa29a0824df | 113 | double MovAvq1_1 = 0.0; |
laurencebr | 0:2aa29a0824df | 114 | double MovAvq1_2 = 0.0; |
laurencebr | 0:2aa29a0824df | 115 | for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted |
laurencebr | 0:2aa29a0824df | 116 | MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1)); |
laurencebr | 0:2aa29a0824df | 117 | MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1)); |
laurencebr | 0:2aa29a0824df | 118 | } |
laurencebr | 0:2aa29a0824df | 119 | DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod; |
laurencebr | 0:2aa29a0824df | 120 | double D_error1 = GainD1 * DerivativeErrorq1; |
laurencebr | 0:2aa29a0824df | 121 | // Derivative error sum over all time? |
laurencebr | 0:2aa29a0824df | 122 | |
laurencebr | 0:2aa29a0824df | 123 | PIDerrorq1 = P_error1 + I_error1 + D_error1; |
laurencebr | 0:2aa29a0824df | 124 | |
laurencebr | 0:2aa29a0824df | 125 | // PID control motor 2 |
laurencebr | 0:2aa29a0824df | 126 | //P-Control |
laurencebr | 0:2aa29a0824df | 127 | double P_error2 = GainP2 * q2Error; |
laurencebr | 0:2aa29a0824df | 128 | |
laurencebr | 0:2aa29a0824df | 129 | //I-Control |
laurencebr | 0:2aa29a0824df | 130 | IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod; |
laurencebr | 0:2aa29a0824df | 131 | double I_error2 = GainI2 * IntegralErrorq2; |
laurencebr | 0:2aa29a0824df | 132 | |
laurencebr | 0:2aa29a0824df | 133 | //D-Control |
laurencebr | 0:2aa29a0824df | 134 | //First smoothen the error signal |
laurencebr | 0:2aa29a0824df | 135 | double MovAvq2_1 = 0.0; |
laurencebr | 0:2aa29a0824df | 136 | double MovAvq2_2 = 0.0; |
laurencebr | 0:2aa29a0824df | 137 | for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted |
laurencebr | 0:2aa29a0824df | 138 | MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1)); |
laurencebr | 0:2aa29a0824df | 139 | MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1)); |
laurencebr | 0:2aa29a0824df | 140 | } |
laurencebr | 0:2aa29a0824df | 141 | DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod; |
laurencebr | 0:2aa29a0824df | 142 | double D_error2 = GainD2 * DerivativeErrorq2; |
laurencebr | 0:2aa29a0824df | 143 | // Derivative error sum over all time? |
laurencebr | 0:2aa29a0824df | 144 | |
laurencebr | 0:2aa29a0824df | 145 | PIDerrorq2 = P_error2 + I_error2 + D_error2; |
laurencebr | 0:2aa29a0824df | 146 | } |
laurencebr | 0:2aa29a0824df | 147 | |
laurencebr | 0:2aa29a0824df | 148 | |
laurencebr | 0:2aa29a0824df | 149 | void MotorControl() |
laurencebr | 0:2aa29a0824df | 150 | { |
laurencebr | 0:2aa29a0824df | 151 | //Motor 1 |
laurencebr | 0:2aa29a0824df | 152 | //Keep signal within bounds |
laurencebr | 0:2aa29a0824df | 153 | if (PIDerrorq1 > 1.0){ |
laurencebr | 0:2aa29a0824df | 154 | PIDerrorq1 = 1.0; |
laurencebr | 0:2aa29a0824df | 155 | } |
laurencebr | 0:2aa29a0824df | 156 | else if (PIDerrorq1 < -1.0){ |
laurencebr | 0:2aa29a0824df | 157 | PIDerrorq1 = -1.0; |
laurencebr | 0:2aa29a0824df | 158 | } |
laurencebr | 0:2aa29a0824df | 159 | //Direction |
laurencebr | 0:2aa29a0824df | 160 | if (PIDerrorq1 <= 0){ |
laurencebr | 0:2aa29a0824df | 161 | Motor1Direction = 0; |
laurencebr | 0:2aa29a0824df | 162 | Motor1PWM.write(-PIDerrorq1); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 163 | } |
laurencebr | 0:2aa29a0824df | 164 | if (PIDerrorq1 >= 0){ |
laurencebr | 0:2aa29a0824df | 165 | Motor1Direction = 1; |
laurencebr | 0:2aa29a0824df | 166 | Motor1PWM.write(PIDerrorq1); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 167 | } |
laurencebr | 0:2aa29a0824df | 168 | |
laurencebr | 0:2aa29a0824df | 169 | //Motor 2 |
laurencebr | 0:2aa29a0824df | 170 | //Keep signal within bounds |
laurencebr | 0:2aa29a0824df | 171 | if (PIDerrorq2 > 1.0){ |
laurencebr | 0:2aa29a0824df | 172 | PIDerrorq2 = 1.0; |
laurencebr | 0:2aa29a0824df | 173 | } |
laurencebr | 0:2aa29a0824df | 174 | else if (PIDerrorq2 < -1.0){ |
laurencebr | 0:2aa29a0824df | 175 | PIDerrorq2 = -1.0; |
laurencebr | 0:2aa29a0824df | 176 | } |
laurencebr | 0:2aa29a0824df | 177 | //Direction |
laurencebr | 0:2aa29a0824df | 178 | |
laurencebr | 0:2aa29a0824df | 179 | if (PIDerrorq2 <= 0){ |
laurencebr | 0:2aa29a0824df | 180 | Motor2Direction = 0; |
laurencebr | 0:2aa29a0824df | 181 | Motor2PWM.write(-PIDerrorq2); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 182 | } |
laurencebr | 0:2aa29a0824df | 183 | if (PIDerrorq2 >= 0){ |
laurencebr | 0:2aa29a0824df | 184 | Motor2Direction = 1; |
laurencebr | 0:2aa29a0824df | 185 | Motor2PWM.write(PIDerrorq2); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 186 | } |
laurencebr | 0:2aa29a0824df | 187 | } |
laurencebr | 0:2aa29a0824df | 188 | |
laurencebr | 0:2aa29a0824df | 189 | void NormalOperatingModus() |
laurencebr | 0:2aa29a0824df | 190 | { |
laurencebr | 0:2aa29a0824df | 191 | UpdateRefPosition(); |
laurencebr | 0:2aa29a0824df | 192 | InverseKinematics(); |
laurencebr | 0:2aa29a0824df | 193 | ReadCurrentPosition(); |
laurencebr | 0:2aa29a0824df | 194 | UpdateError(); |
laurencebr | 0:2aa29a0824df | 195 | PIDControl(); |
laurencebr | 0:2aa29a0824df | 196 | MotorControl(); |
laurencebr | 0:2aa29a0824df | 197 | |
laurencebr | 0:2aa29a0824df | 198 | scope.set(0, q1Pos); |
laurencebr | 0:2aa29a0824df | 199 | scope.set(1, q1Ref); |
laurencebr | 0:2aa29a0824df | 200 | |
laurencebr | 0:2aa29a0824df | 201 | GainP1 = pot3.read() * 10; |
laurencebr | 0:2aa29a0824df | 202 | GainI1 = pot2.read() * 10; |
laurencebr | 0:2aa29a0824df | 203 | GainD1 = pot1.read() ; |
laurencebr | 0:2aa29a0824df | 204 | |
laurencebr | 0:2aa29a0824df | 205 | countstep++; |
laurencebr | 0:2aa29a0824df | 206 | count++; |
laurencebr | 0:2aa29a0824df | 207 | if (count == 400) // print 1x per seconde waardes. |
laurencebr | 0:2aa29a0824df | 208 | { |
laurencebr | 0:2aa29a0824df | 209 | pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref); |
laurencebr | 0:2aa29a0824df | 210 | count = 0; |
laurencebr | 0:2aa29a0824df | 211 | } |
laurencebr | 0:2aa29a0824df | 212 | if (countstep == 4000) |
laurencebr | 0:2aa29a0824df | 213 | { |
laurencebr | 0:2aa29a0824df | 214 | q1Ref = !q1Ref; |
laurencebr | 0:2aa29a0824df | 215 | countstep = 0; |
laurencebr | 0:2aa29a0824df | 216 | } |
laurencebr | 0:2aa29a0824df | 217 | |
laurencebr | 0:2aa29a0824df | 218 | |
laurencebr | 0:2aa29a0824df | 219 | } |
laurencebr | 0:2aa29a0824df | 220 | |
laurencebr | 0:2aa29a0824df | 221 | |
laurencebr | 0:2aa29a0824df | 222 | int main() |
laurencebr | 0:2aa29a0824df | 223 | { |
laurencebr | 0:2aa29a0824df | 224 | pc.baud(115200); |
laurencebr | 0:2aa29a0824df | 225 | //Initialize array errors to 0 |
laurencebr | 0:2aa29a0824df | 226 | for (int i = 0; i <= 9; i++){ |
laurencebr | 0:2aa29a0824df | 227 | PrevErrorq2[i] = 0; |
laurencebr | 0:2aa29a0824df | 228 | PrevErrorq2[i] = 0; |
laurencebr | 0:2aa29a0824df | 229 | } |
laurencebr | 0:2aa29a0824df | 230 | |
laurencebr | 0:2aa29a0824df | 231 | int frequency_pwm = 16700; //16.7 kHz PWM |
laurencebr | 0:2aa29a0824df | 232 | Motor1PWM.period(1.0/frequency_pwm); // T = 1/f |
laurencebr | 0:2aa29a0824df | 233 | Motor2PWM.period(1.0/frequency_pwm); // T = 1/f |
laurencebr | 0:2aa29a0824df | 234 | |
laurencebr | 0:2aa29a0824df | 235 | Kikker.attach(NormalOperatingModus, TickerPeriod); |
laurencebr | 0:2aa29a0824df | 236 | scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); |
laurencebr | 0:2aa29a0824df | 237 | |
laurencebr | 0:2aa29a0824df | 238 | while(true); |
laurencebr | 0:2aa29a0824df | 239 | {} |
laurencebr | 0:2aa29a0824df | 240 | } |
laurencebr | 0:2aa29a0824df | 241 | |
laurencebr | 0:2aa29a0824df | 242 | |
laurencebr | 0:2aa29a0824df | 243 | |
laurencebr | 0:2aa29a0824df | 244 |