Laurence B / Mbed 2 deprecated deelPID

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?

UserRevisionLine numberNew 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