M9 Biorobotics Group 8 / Mbed 2 deprecated MotorControlv2

Motor control

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@18:e0086bd1d87e, 2019-10-21 (annotated)

Committer:
BasB
Date:
Mon Oct 21 14:52:06 2019 +0000
Revision:
18:e0086bd1d87e
Parent:
17:8a0c720733b8 
PID control met RKI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
freek100 0:e4858e2df9c7 1 #include "mbed.h"
freek100 3:07fedd2e252c 2 #include "HIDScope.h"
freek100 0:e4858e2df9c7 3 #include "QEI.h"
freek100 0:e4858e2df9c7 4 #include "MODSERIAL.h"
freek100 3:07fedd2e252c 5 #include "BiQuad.h"
freek100 0:e4858e2df9c7 6 #include "FastPWM.h"
freek100 0:e4858e2df9c7 7
freek100 6:1c0b6e55e900 8 // Button and potmeter1 control
freek100 0:e4858e2df9c7 9 InterruptIn button1(D11);
freek100 0:e4858e2df9c7 10 InterruptIn button2(D10);
BasB 10:b871d1b05787 11 InterruptIn buttonsw2(SW2);
BasB 10:b871d1b05787 12 InterruptIn buttonsw3(SW3);
freek100 6:1c0b6e55e900 13 AnalogIn potmeter1(A0);
freek100 6:1c0b6e55e900 14 AnalogIn potmeter2(A1);
freek100 8:c6c94d55b088 15 AnalogIn potmeter3(A2);
BasB 9:08a7a8e59a6a 16 AnalogIn potmeter4(A3);
freek100 0:e4858e2df9c7 17 // Encoder
BasB 15:fb0bbce41a0f 18 DigitalIn encA1(D9);
BasB 15:fb0bbce41a0f 19 DigitalIn encB1(D8);
BasB 15:fb0bbce41a0f 20 DigitalIn encA2(D13);
BasB 15:fb0bbce41a0f 21 DigitalIn encB2(D13);
BasB 15:fb0bbce41a0f 22 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);
BasB 15:fb0bbce41a0f 23 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);
freek100 2:d7286c36595f 24 float Ts = 0.01;
BasB 13:048458947701 25 float motor1angle;
BasB 14:0afc46ad1b99 26 float motor2angle;
BasB 13:048458947701 27 float omega1;
BasB 14:0afc46ad1b99 28 float omega2;
freek100 1:08e8cc33fcae 29
freek100 0:e4858e2df9c7 30
freek100 0:e4858e2df9c7 31 // Motor
freek100 0:e4858e2df9c7 32 DigitalOut motor2Direction(D4);
freek100 0:e4858e2df9c7 33 FastPWM motor2Power(D5);
freek100 0:e4858e2df9c7 34 DigitalOut motor1Direction(D7);
freek100 0:e4858e2df9c7 35 FastPWM motor1Power(D6);
freek100 0:e4858e2df9c7 36
BasB 17:8a0c720733b8 37 volatile int motortoggle = 1;
freek100 5:17aa878564d0 38
freek100 0:e4858e2df9c7 39 //Motorcontrol
BasB 13:048458947701 40 bool motordir1;
BasB 14:0afc46ad1b99 41 bool motordir2;
BasB 17:8a0c720733b8 42 float motor1ref=0.1745;
BasB 17:8a0c720733b8 43 float motor2ref=0.0873;
BasB 13:048458947701 44 double controlsignal1;
BasB 14:0afc46ad1b99 45 double controlsignal2;
freek100 1:08e8cc33fcae 46 double pi2= 6.283185;
BasB 13:048458947701 47 float motor1error; //e = error
BasB 14:0afc46ad1b99 48 float motor2error;
BasB 13:048458947701 49 float Kp=0.27;
BasB 13:048458947701 50 float Ki=0.35;
BasB 16:e51ddfaf2e7a 51 float Kd=0.1;
BasB 13:048458947701 52 float u_p1;
BasB 14:0afc46ad1b99 53 float u_p2;
BasB 13:048458947701 54 float u_i1;
BasB 14:0afc46ad1b99 55 float u_i2;
freek100 3:07fedd2e252c 56
freek100 11:94a4dd7ed05c 57 //Windup control
BasB 13:048458947701 58 float ux1;
BasB 14:0afc46ad1b99 59 float ux2;
BasB 13:048458947701 60 float up1;
BasB 14:0afc46ad1b99 61 float up2;
BasB 13:048458947701 62 float ek1;
BasB 14:0afc46ad1b99 63 float ek2;
BasB 17:8a0c720733b8 64 float ei1= 0.0;
BasB 17:8a0c720733b8 65 float ei2=0.0;
BasB 17:8a0c720733b8 66 float Ka= 1.0;
BasB 17:8a0c720733b8 67
BasB 17:8a0c720733b8 68 //RKI
BasB 17:8a0c720733b8 69 float Vx;
BasB 17:8a0c720733b8 70 float Vy;
BasB 17:8a0c720733b8 71 float q1dot;
BasB 17:8a0c720733b8 72 float q2dot;
BasB 17:8a0c720733b8 73 float l1=26.0; //Afstand base-link [cm]
BasB 17:8a0c720733b8 74 float l2=62.0; //Afstand link-endpoint [cm]
freek100 11:94a4dd7ed05c 75
freek100 4:e7d50c6a7c53 76 //Hidscope
BasB 18:e0086bd1d87e 77 HIDScope scope(4); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
BasB 14:0afc46ad1b99 78
freek100 0:e4858e2df9c7 79 // PC connection
freek100 0:e4858e2df9c7 80 MODSERIAL pc(USBTX, USBRX);
freek100 0:e4858e2df9c7 81
freek100 0:e4858e2df9c7 82 // Intializing tickers
freek100 0:e4858e2df9c7 83 Ticker motorTicker;
freek100 0:e4858e2df9c7 84 Ticker controlTicker;
freek100 0:e4858e2df9c7 85 Ticker directionTicker;
freek100 1:08e8cc33fcae 86 Ticker encoderTicker;
freek100 4:e7d50c6a7c53 87 Ticker scopeTicker;
freek100 0:e4858e2df9c7 88
freek100 0:e4858e2df9c7 89 const float PWM_period = 1e-6;
freek100 0:e4858e2df9c7 90
BasB 13:048458947701 91 volatile int counts1; // Encoder counts
BasB 14:0afc46ad1b99 92 volatile int counts2;
BasB 13:048458947701 93 volatile int countsPrev1 = 0;
BasB 14:0afc46ad1b99 94 volatile int countsPrev2 = 0;
BasB 13:048458947701 95 volatile int deltaCounts1;
BasB 14:0afc46ad1b99 96 volatile int deltaCounts2;
freek100 0:e4858e2df9c7 97
freek100 0:e4858e2df9c7 98 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
freek100 0:e4858e2df9c7 99 float gearratio = 131.25; // Gear ratio of gearbox
freek100 0:e4858e2df9c7 100
freek100 2:d7286c36595f 101
BasB 13:048458947701 102 float PID_controller1(float motor1error){
BasB 13:048458947701 103 static float error_integral1=0;
BasB 16:e51ddfaf2e7a 104 static float e_prev1=motor1error;
freek100 2:d7286c36595f 105
freek100 2:d7286c36595f 106 //Proportional part:
BasB 13:048458947701 107 u_p1=Kp*motor1error;
freek100 2:d7286c36595f 108
freek100 2:d7286c36595f 109 //Integral part
BasB 13:048458947701 110 error_integral1=error_integral1+ei1*Ts;
BasB 13:048458947701 111 u_i1=Ki*error_integral1;
freek100 2:d7286c36595f 112
BasB 16:e51ddfaf2e7a 113 //Derivative part
BasB 16:e51ddfaf2e7a 114 float error_derivative1=(motor1error-e_prev1)/Ts;
BasB 16:e51ddfaf2e7a 115 float u_d1=Kd*error_derivative1;
BasB 16:e51ddfaf2e7a 116 e_prev1=motor1error;
BasB 16:e51ddfaf2e7a 117
freek100 11:94a4dd7ed05c 118 // Sum and limit
BasB 16:e51ddfaf2e7a 119 up1= u_p1+u_i1+u_d1;
BasB 13:048458947701 120 if (up1>1){
BasB 13:048458947701 121 controlsignal1=1;}
BasB 13:048458947701 122 else if (up1<-1){
BasB 13:048458947701 123 controlsignal1=-1;}
freek100 11:94a4dd7ed05c 124 else {
BasB 13:048458947701 125 controlsignal1=up1;}
freek100 11:94a4dd7ed05c 126
freek100 11:94a4dd7ed05c 127 // To prevent windup
BasB 13:048458947701 128 ux1= up1-controlsignal1;
BasB 13:048458947701 129 ek1= Ka*ux1;
BasB 13:048458947701 130 ei1= motor1error-ek1;
freek100 11:94a4dd7ed05c 131 //Return
BasB 13:048458947701 132 return controlsignal1;
freek100 2:d7286c36595f 133 }
freek100 2:d7286c36595f 134
BasB 14:0afc46ad1b99 135 float PID_controller2(float motor2error){
BasB 14:0afc46ad1b99 136 static float error_integral2=0;
BasB 16:e51ddfaf2e7a 137 static float e_prev2=motor2error;
BasB 14:0afc46ad1b99 138
BasB 14:0afc46ad1b99 139 //Proportional part:
BasB 14:0afc46ad1b99 140 u_p2=Kp*motor2error;
BasB 14:0afc46ad1b99 141
BasB 14:0afc46ad1b99 142 //Integral part
BasB 14:0afc46ad1b99 143 error_integral2=error_integral2+ei2*Ts;
BasB 14:0afc46ad1b99 144 u_i2=Ki*error_integral2;
BasB 14:0afc46ad1b99 145
BasB 16:e51ddfaf2e7a 146 //Derivative part
BasB 16:e51ddfaf2e7a 147 float error_derivative2=(motor2error-e_prev2)/Ts;
BasB 16:e51ddfaf2e7a 148 float u_d2=Kd*error_derivative2;
BasB 16:e51ddfaf2e7a 149 e_prev2=motor2error;
BasB 16:e51ddfaf2e7a 150
BasB 14:0afc46ad1b99 151 // Sum and limit
BasB 16:e51ddfaf2e7a 152 up2= u_p2+u_i2+u_d2;
BasB 14:0afc46ad1b99 153 if (up2>1){
BasB 14:0afc46ad1b99 154 controlsignal2=1;}
BasB 14:0afc46ad1b99 155 else if (up2<-1){
BasB 14:0afc46ad1b99 156 controlsignal2=-1;}
BasB 14:0afc46ad1b99 157 else {
BasB 14:0afc46ad1b99 158 controlsignal2=up2;}
BasB 14:0afc46ad1b99 159
BasB 14:0afc46ad1b99 160 // To prevent windup
BasB 14:0afc46ad1b99 161 ux2= up2-controlsignal2;
BasB 14:0afc46ad1b99 162 ek2= Ka*ux2;
BasB 14:0afc46ad1b99 163 ei2= motor2error-ek2;
BasB 14:0afc46ad1b99 164 //Return
BasB 14:0afc46ad1b99 165 return controlsignal2;
BasB 14:0afc46ad1b99 166 }
BasB 14:0afc46ad1b99 167
freek100 4:e7d50c6a7c53 168
freek100 4:e7d50c6a7c53 169 void readEncoder()
freek100 4:e7d50c6a7c53 170 {
BasB 14:0afc46ad1b99 171 counts1 = encoder1.getPulses();
BasB 13:048458947701 172 deltaCounts1 = counts1 - countsPrev1;
BasB 13:048458947701 173 countsPrev1 = counts1;
BasB 14:0afc46ad1b99 174
BasB 14:0afc46ad1b99 175 counts2 = encoder2.getPulses();
BasB 14:0afc46ad1b99 176 deltaCounts2 = counts2 - countsPrev2;
BasB 14:0afc46ad1b99 177 countsPrev2 = counts2;
freek100 4:e7d50c6a7c53 178 }
BasB 9:08a7a8e59a6a 179
BasB 9:08a7a8e59a6a 180 void togglehoek(){
freek100 11:94a4dd7ed05c 181
BasB 13:048458947701 182 motor1ref= 0.5*pi2+motor1ref;
BasB 14:0afc46ad1b99 183 motor2ref= 0.5*pi2+motor2ref;
freek100 11:94a4dd7ed05c 184 // static float t = 0;
BasB 17:8a0c720733b8 185 // refangle= pi2/3.0f*sin(5.0f*t)*motortoggle;
freek100 11:94a4dd7ed05c 186 //t+=0.01;
BasB 9:08a7a8e59a6a 187 }
BasB 16:e51ddfaf2e7a 188
BasB 17:8a0c720733b8 189 void RKI(){
BasB 17:8a0c720733b8 190
BasB 17:8a0c720733b8 191 Vy=potmeter1.read()*-10.0*motortoggle;
BasB 17:8a0c720733b8 192 //Vy=potmeter2.read()*10*motortoggle;
BasB 17:8a0c720733b8 193 //Vx=-1.0*;
BasB 17:8a0c720733b8 194 Vx=0.0;
BasB 17:8a0c720733b8 195 q1dot=(l2*cos(motor1ref+motor2ref)*Vx+l2*sin(motor1ref+motor2ref)*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref)));
BasB 17:8a0c720733b8 196 q2dot=((-l1*cos(motor1ref)-l2*cos(motor1ref+motor2ref))*Vx+(-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref)));
BasB 17:8a0c720733b8 197
BasB 17:8a0c720733b8 198 motor1ref=motor1ref+q1dot*Ts;
BasB 17:8a0c720733b8 199 motor2ref=motor2ref+q2dot*Ts;
BasB 17:8a0c720733b8 200 }
BasB 9:08a7a8e59a6a 201
freek100 0:e4858e2df9c7 202 void motorControl()
freek100 0:e4858e2df9c7 203 {
freek100 11:94a4dd7ed05c 204 button1.fall(&togglehoek);
BasB 17:8a0c720733b8 205 RKI();
BasB 13:048458947701 206 motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad
BasB 13:048458947701 207 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 14:0afc46ad1b99 208 motor1error=motor1ref-motor1angle;
BasB 14:0afc46ad1b99 209 controlsignal1=PID_controller1(motor1error);
BasB 13:048458947701 210 if (controlsignal1<0){
BasB 13:048458947701 211 motordir1= 0;}
BasB 10:b871d1b05787 212 else {
BasB 13:048458947701 213 motordir1= 1;}
BasB 13:048458947701 214 motor1Power.write(abs(controlsignal1));
BasB 13:048458947701 215 motor1Direction= motordir1;
BasB 14:0afc46ad1b99 216
BasB 14:0afc46ad1b99 217 motor2angle = counts2 * factorin / gearratio; // Angle of motor shaft in rad
BasB 14:0afc46ad1b99 218 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 14:0afc46ad1b99 219 motor2error=motor2ref-motor2angle;
BasB 14:0afc46ad1b99 220 controlsignal2=PID_controller2(motor2error);
BasB 14:0afc46ad1b99 221 if (controlsignal2<0){
BasB 14:0afc46ad1b99 222 motordir2= 0;}
BasB 14:0afc46ad1b99 223 else {
BasB 14:0afc46ad1b99 224 motordir2= 1;}
BasB 14:0afc46ad1b99 225 motor2Power.write(abs(controlsignal2));
BasB 14:0afc46ad1b99 226 motor2Direction= motordir2;
freek100 0:e4858e2df9c7 227 }
freek100 0:e4858e2df9c7 228
BasB 10:b871d1b05787 229 void Plotje(){
BasB 13:048458947701 230 scope.set(0,motor1ref); //gewenste hoek
BasB 17:8a0c720733b8 231 scope.set(1,motor2ref);
BasB 17:8a0c720733b8 232 scope.set(2,motor1angle); //Gemeten hoek
BasB 17:8a0c720733b8 233 scope.set(3,motor1error); //verschil in gewenste en gemeten hoek
freek100 1:08e8cc33fcae 234
freek100 4:e7d50c6a7c53 235 scope.send(); //send what's in scope memory to PC
freek100 1:08e8cc33fcae 236 }
freek100 0:e4858e2df9c7 237
freek100 5:17aa878564d0 238 void toggleMotor()
freek100 5:17aa878564d0 239 {
BasB 17:8a0c720733b8 240 motortoggle = !motortoggle;
freek100 5:17aa878564d0 241 }
freek100 4:e7d50c6a7c53 242
freek100 0:e4858e2df9c7 243 int main()
freek100 0:e4858e2df9c7 244 {
freek100 0:e4858e2df9c7 245 pc.baud(115200);
freek100 0:e4858e2df9c7 246 pc.printf("\r\nStarting...\r\n\r\n");
freek100 0:e4858e2df9c7 247
freek100 0:e4858e2df9c7 248 motor1Power.period(PWM_period);
BasB 14:0afc46ad1b99 249 motor2Power.period(PWM_period);
freek100 0:e4858e2df9c7 250 motorTicker.attach(motorControl, 0.01);
freek100 4:e7d50c6a7c53 251 scopeTicker.attach(Plotje, 0.01);
freek100 2:d7286c36595f 252 encoderTicker.attach(readEncoder, Ts);
freek100 5:17aa878564d0 253
freek100 5:17aa878564d0 254 button2.fall(&toggleMotor);
BasB 9:08a7a8e59a6a 255
freek100 0:e4858e2df9c7 256 while (true) {
BasB 17:8a0c720733b8 257 // pc.printf("Angle1: %f Omega1: %f\r\n", motor1angle, omega1);
BasB 14:0afc46ad1b99 258 pc.printf("refangle1: %f Error1: %f \r\n",motor1ref, motor1error);
BasB 17:8a0c720733b8 259 // pc.printf("Angle2: %f Omega2: %f\r\n", motor2angle, omega2);
BasB 14:0afc46ad1b99 260 pc.printf("refangle2: %f Error2: %f \r\n",motor2ref, motor2error);
BasB 17:8a0c720733b8 261 // pc.printf("controlsignal2: %d \r\n", controlsignal2);
BasB 17:8a0c720733b8 262 pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy);
BasB 17:8a0c720733b8 263 pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot);
freek100 6:1c0b6e55e900 264
freek100 0:e4858e2df9c7 265 wait(0.5);
freek100 0:e4858e2df9c7 266 }
freek100 0:e4858e2df9c7 267 }