M9 Biorobotics Group 8 / Mbed 2 deprecated MotorControlv2

Motor control

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@15:fb0bbce41a0f, 2019-10-21 (annotated)

Committer:
BasB
Date:
Mon Oct 21 10:05:12 2019 +0000
Revision:
15:fb0bbce41a0f
Parent:
14:0afc46ad1b99
Child:
16:e51ddfaf2e7a
Beide motoren werkend met PI control

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