M9 Biorobotics Group 8 / Mbed 2 deprecated MotorControlv2

Motor control

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@16:e51ddfaf2e7a, 2019-10-21 (annotated)

Committer:
BasB
Date:
Mon Oct 21 12:43:43 2019 +0000
Revision:
16:e51ddfaf2e7a
Parent:
15:fb0bbce41a0f
Child:
17:8a0c720733b8
Toch ineens weer de Kd (derivative)  toegevoegd omdat CRS man dat zei

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
freek100 5:17aa878564d0 37 volatile int motor1Toggle = 1;
freek100 5:17aa878564d0 38
freek100 0:e4858e2df9c7 39 //Motorcontrol
BasB 13:048458947701 40 bool motordir1;
BasB 14:0afc46ad1b99 41 bool motordir2;
BasB 13:048458947701 42 float motor1ref= 0;
BasB 14:0afc46ad1b99 43 float motor2ref=0;
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 13:048458947701 64 float ei1= 0;
BasB 14:0afc46ad1b99 65 float ei2=0;
freek100 11:94a4dd7ed05c 66 float Ka= 1;
freek100 11:94a4dd7ed05c 67
freek100 4:e7d50c6a7c53 68 //Hidscope
freek100 4:e7d50c6a7c53 69 HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
BasB 14:0afc46ad1b99 70
freek100 0:e4858e2df9c7 71 // PC connection
freek100 0:e4858e2df9c7 72 MODSERIAL pc(USBTX, USBRX);
freek100 0:e4858e2df9c7 73
freek100 0:e4858e2df9c7 74 // Intializing tickers
freek100 0:e4858e2df9c7 75 Ticker motorTicker;
freek100 0:e4858e2df9c7 76 Ticker controlTicker;
freek100 0:e4858e2df9c7 77 Ticker directionTicker;
freek100 1:08e8cc33fcae 78 Ticker encoderTicker;
freek100 4:e7d50c6a7c53 79 Ticker scopeTicker;
freek100 0:e4858e2df9c7 80
freek100 0:e4858e2df9c7 81 const float PWM_period = 1e-6;
freek100 0:e4858e2df9c7 82
BasB 13:048458947701 83 volatile int counts1; // Encoder counts
BasB 14:0afc46ad1b99 84 volatile int counts2;
BasB 13:048458947701 85 volatile int countsPrev1 = 0;
BasB 14:0afc46ad1b99 86 volatile int countsPrev2 = 0;
BasB 13:048458947701 87 volatile int deltaCounts1;
BasB 14:0afc46ad1b99 88 volatile int deltaCounts2;
freek100 0:e4858e2df9c7 89
freek100 0:e4858e2df9c7 90 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
freek100 0:e4858e2df9c7 91 float gearratio = 131.25; // Gear ratio of gearbox
freek100 0:e4858e2df9c7 92
freek100 2:d7286c36595f 93
BasB 13:048458947701 94 float PID_controller1(float motor1error){
BasB 13:048458947701 95 static float error_integral1=0;
BasB 16:e51ddfaf2e7a 96 static float e_prev1=motor1error;
freek100 2:d7286c36595f 97
freek100 2:d7286c36595f 98 //Proportional part:
BasB 13:048458947701 99 u_p1=Kp*motor1error;
freek100 2:d7286c36595f 100
freek100 2:d7286c36595f 101 //Integral part
BasB 13:048458947701 102 error_integral1=error_integral1+ei1*Ts;
BasB 13:048458947701 103 u_i1=Ki*error_integral1;
freek100 2:d7286c36595f 104
BasB 16:e51ddfaf2e7a 105 //Derivative part
BasB 16:e51ddfaf2e7a 106 float error_derivative1=(motor1error-e_prev1)/Ts;
BasB 16:e51ddfaf2e7a 107 float u_d1=Kd*error_derivative1;
BasB 16:e51ddfaf2e7a 108 e_prev1=motor1error;
BasB 16:e51ddfaf2e7a 109
freek100 11:94a4dd7ed05c 110 // Sum and limit
BasB 16:e51ddfaf2e7a 111 up1= u_p1+u_i1+u_d1;
BasB 13:048458947701 112 if (up1>1){
BasB 13:048458947701 113 controlsignal1=1;}
BasB 13:048458947701 114 else if (up1<-1){
BasB 13:048458947701 115 controlsignal1=-1;}
freek100 11:94a4dd7ed05c 116 else {
BasB 13:048458947701 117 controlsignal1=up1;}
freek100 11:94a4dd7ed05c 118
freek100 11:94a4dd7ed05c 119 // To prevent windup
BasB 13:048458947701 120 ux1= up1-controlsignal1;
BasB 13:048458947701 121 ek1= Ka*ux1;
BasB 13:048458947701 122 ei1= motor1error-ek1;
freek100 11:94a4dd7ed05c 123 //Return
BasB 13:048458947701 124 return controlsignal1;
freek100 2:d7286c36595f 125 }
freek100 2:d7286c36595f 126
BasB 14:0afc46ad1b99 127 float PID_controller2(float motor2error){
BasB 14:0afc46ad1b99 128 static float error_integral2=0;
BasB 16:e51ddfaf2e7a 129 static float e_prev2=motor2error;
BasB 14:0afc46ad1b99 130
BasB 14:0afc46ad1b99 131 //Proportional part:
BasB 14:0afc46ad1b99 132 u_p2=Kp*motor2error;
BasB 14:0afc46ad1b99 133
BasB 14:0afc46ad1b99 134 //Integral part
BasB 14:0afc46ad1b99 135 error_integral2=error_integral2+ei2*Ts;
BasB 14:0afc46ad1b99 136 u_i2=Ki*error_integral2;
BasB 14:0afc46ad1b99 137
BasB 16:e51ddfaf2e7a 138 //Derivative part
BasB 16:e51ddfaf2e7a 139 float error_derivative2=(motor2error-e_prev2)/Ts;
BasB 16:e51ddfaf2e7a 140 float u_d2=Kd*error_derivative2;
BasB 16:e51ddfaf2e7a 141 e_prev2=motor2error;
BasB 16:e51ddfaf2e7a 142
BasB 14:0afc46ad1b99 143 // Sum and limit
BasB 16:e51ddfaf2e7a 144 up2= u_p2+u_i2+u_d2;
BasB 14:0afc46ad1b99 145 if (up2>1){
BasB 14:0afc46ad1b99 146 controlsignal2=1;}
BasB 14:0afc46ad1b99 147 else if (up2<-1){
BasB 14:0afc46ad1b99 148 controlsignal2=-1;}
BasB 14:0afc46ad1b99 149 else {
BasB 14:0afc46ad1b99 150 controlsignal2=up2;}
BasB 14:0afc46ad1b99 151
BasB 14:0afc46ad1b99 152 // To prevent windup
BasB 14:0afc46ad1b99 153 ux2= up2-controlsignal2;
BasB 14:0afc46ad1b99 154 ek2= Ka*ux2;
BasB 14:0afc46ad1b99 155 ei2= motor2error-ek2;
BasB 14:0afc46ad1b99 156 //Return
BasB 14:0afc46ad1b99 157 return controlsignal2;
BasB 14:0afc46ad1b99 158 }
BasB 14:0afc46ad1b99 159
freek100 4:e7d50c6a7c53 160
freek100 4:e7d50c6a7c53 161 void readEncoder()
freek100 4:e7d50c6a7c53 162 {
BasB 14:0afc46ad1b99 163 counts1 = encoder1.getPulses();
BasB 13:048458947701 164 deltaCounts1 = counts1 - countsPrev1;
BasB 13:048458947701 165 countsPrev1 = counts1;
BasB 14:0afc46ad1b99 166
BasB 14:0afc46ad1b99 167 counts2 = encoder2.getPulses();
BasB 14:0afc46ad1b99 168 deltaCounts2 = counts2 - countsPrev2;
BasB 14:0afc46ad1b99 169 countsPrev2 = counts2;
freek100 4:e7d50c6a7c53 170 }
BasB 9:08a7a8e59a6a 171
BasB 9:08a7a8e59a6a 172 void togglehoek(){
freek100 11:94a4dd7ed05c 173
BasB 13:048458947701 174 motor1ref= 0.5*pi2+motor1ref;
BasB 14:0afc46ad1b99 175 motor2ref= 0.5*pi2+motor2ref;
freek100 11:94a4dd7ed05c 176 // static float t = 0;
freek100 11:94a4dd7ed05c 177 // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
freek100 11:94a4dd7ed05c 178 //t+=0.01;
BasB 9:08a7a8e59a6a 179 }
BasB 16:e51ddfaf2e7a 180
BasB 16:e51ddfaf2e7a 181 //void RKI(){
BasB 16:e51ddfaf2e7a 182
BasB 16:e51ddfaf2e7a 183 // RKI magic
BasB 16:e51ddfaf2e7a 184
BasB 16:e51ddfaf2e7a 185 // motor1ref=??;
BasB 16:e51ddfaf2e7a 186 // motor2ref=??;
BasB 16:e51ddfaf2e7a 187 // }
BasB 9:08a7a8e59a6a 188
freek100 0:e4858e2df9c7 189 void motorControl()
freek100 0:e4858e2df9c7 190 {
freek100 11:94a4dd7ed05c 191 button1.fall(&togglehoek);
BasB 16:e51ddfaf2e7a 192 // RKI()
BasB 13:048458947701 193 motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad
BasB 13:048458947701 194 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 14:0afc46ad1b99 195 motor1error=motor1ref-motor1angle;
BasB 14:0afc46ad1b99 196 controlsignal1=PID_controller1(motor1error);
BasB 13:048458947701 197 if (controlsignal1<0){
BasB 13:048458947701 198 motordir1= 0;}
BasB 10:b871d1b05787 199 else {
BasB 13:048458947701 200 motordir1= 1;}
BasB 13:048458947701 201 motor1Power.write(abs(controlsignal1));
BasB 13:048458947701 202 motor1Direction= motordir1;
BasB 14:0afc46ad1b99 203
BasB 14:0afc46ad1b99 204 motor2angle = counts2 * factorin / gearratio; // Angle of motor shaft in rad
BasB 14:0afc46ad1b99 205 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 14:0afc46ad1b99 206 motor2error=motor2ref-motor2angle;
BasB 14:0afc46ad1b99 207 controlsignal2=PID_controller2(motor2error);
BasB 14:0afc46ad1b99 208 if (controlsignal2<0){
BasB 14:0afc46ad1b99 209 motordir2= 0;}
BasB 14:0afc46ad1b99 210 else {
BasB 14:0afc46ad1b99 211 motordir2= 1;}
BasB 14:0afc46ad1b99 212 motor2Power.write(abs(controlsignal2));
BasB 14:0afc46ad1b99 213 motor2Direction= motordir2;
freek100 0:e4858e2df9c7 214 }
freek100 0:e4858e2df9c7 215
BasB 10:b871d1b05787 216 void Plotje(){
BasB 13:048458947701 217 scope.set(0,motor1ref); //gewenste hoek
BasB 13:048458947701 218 scope.set(1,motor1angle); //Gemeten hoek
BasB 13:048458947701 219 scope.set(2,motor1error); //verschil in gewenste en gemeten hoek
freek100 1:08e8cc33fcae 220
freek100 4:e7d50c6a7c53 221 scope.send(); //send what's in scope memory to PC
freek100 1:08e8cc33fcae 222 }
freek100 0:e4858e2df9c7 223
freek100 5:17aa878564d0 224 void toggleMotor()
freek100 5:17aa878564d0 225 {
freek100 5:17aa878564d0 226 motor1Toggle = !motor1Toggle;
freek100 5:17aa878564d0 227 }
freek100 4:e7d50c6a7c53 228
freek100 0:e4858e2df9c7 229 int main()
freek100 0:e4858e2df9c7 230 {
freek100 0:e4858e2df9c7 231 pc.baud(115200);
freek100 0:e4858e2df9c7 232 pc.printf("\r\nStarting...\r\n\r\n");
freek100 0:e4858e2df9c7 233
freek100 0:e4858e2df9c7 234 motor1Power.period(PWM_period);
BasB 14:0afc46ad1b99 235 motor2Power.period(PWM_period);
freek100 0:e4858e2df9c7 236 motorTicker.attach(motorControl, 0.01);
freek100 4:e7d50c6a7c53 237 scopeTicker.attach(Plotje, 0.01);
freek100 2:d7286c36595f 238 encoderTicker.attach(readEncoder, Ts);
freek100 5:17aa878564d0 239
freek100 5:17aa878564d0 240 button2.fall(&toggleMotor);
BasB 9:08a7a8e59a6a 241
freek100 0:e4858e2df9c7 242 while (true) {
BasB 14:0afc46ad1b99 243 pc.printf("Angle1: %f Omega1: %f\r\n", motor1angle, omega1);
BasB 14:0afc46ad1b99 244 pc.printf("refangle1: %f Error1: %f \r\n",motor1ref, motor1error);
BasB 14:0afc46ad1b99 245 pc.printf("Angle2: %f Omega2: %f\r\n", motor2angle, omega2);
BasB 14:0afc46ad1b99 246 pc.printf("refangle2: %f Error2: %f \r\n",motor2ref, motor2error);
BasB 15:fb0bbce41a0f 247 pc.printf("controlsignal2: %d \r\n", controlsignal2);
freek100 6:1c0b6e55e900 248
freek100 0:e4858e2df9c7 249 wait(0.5);
freek100 0:e4858e2df9c7 250 }
freek100 0:e4858e2df9c7 251 }