![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
Motor control
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 14:0afc46ad1b99
- Parent:
- 13:048458947701
- Child:
- 15:fb0bbce41a0f
diff -r 048458947701 -r 0afc46ad1b99 main.cpp --- a/main.cpp Mon Oct 21 08:57:45 2019 +0000 +++ b/main.cpp Mon Oct 21 09:39:10 2019 +0000 @@ -17,10 +17,15 @@ // Encoder DigitalIn encA1(D13); DigitalIn encB1(D12); +DigitalIn encA2(D9); +DigitalIn encB2(D8); QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING); +QEI encoder2(D9,D8,NC,64,QEI::X4_ENCODING); float Ts = 0.01; float motor1angle; +float motor2angle; float omega1; +float omega2; // Motor @@ -33,25 +38,35 @@ //Motorcontrol bool motordir1; +bool motordir2; float motor1ref= 0; +float motor2ref=0; double controlsignal1; -double potValue; +double controlsignal2; double pi2= 6.283185; float motor1error; //e = error +float motor2error; float Kp=0.27; float Ki=0.35; float u_p1; +float u_p2; float u_i1; +float u_i2; //Windup control float ux1; +float ux2; float up1; +float up2; float ek1; +float ek2; float ei1= 0; +float ei2=0; float Ka= 1; //Hidscope HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application. + // PC connection MODSERIAL pc(USBTX, USBRX); @@ -65,8 +80,11 @@ const float PWM_period = 1e-6; volatile int counts1; // Encoder counts +volatile int counts2; volatile int countsPrev1 = 0; +volatile int countsPrev2 = 0; volatile int deltaCounts1; +volatile int deltaCounts2; float factorin = 6.23185/64; // Convert encoder counts to angle in rad float gearratio = 131.25; // Gear ratio of gearbox @@ -100,18 +118,49 @@ return controlsignal1; } +float PID_controller2(float motor2error){ + static float error_integral2=0; + + //Proportional part: + u_p2=Kp*motor2error; + + //Integral part + error_integral2=error_integral2+ei2*Ts; + u_i2=Ki*error_integral2; + + // Sum and limit + up2= u_p2+u_i2; + if (up2>1){ + controlsignal2=1;} + else if (up2<-1){ + controlsignal2=-1;} + else { + controlsignal2=up2;} + + // To prevent windup + ux2= up2-controlsignal2; + ek2= Ka*ux2; + ei2= motor2error-ek2; + //Return + return controlsignal2; +} + void readEncoder() { - counts1 = encoder1.getPulses(); + counts1 = encoder1.getPulses(); deltaCounts1 = counts1 - countsPrev1; - countsPrev1 = counts1; + + counts2 = encoder2.getPulses(); + deltaCounts2 = counts2 - countsPrev2; + countsPrev2 = counts2; } void togglehoek(){ motor1ref= 0.5*pi2+motor1ref; + motor2ref= 0.5*pi2+motor2ref; // static float t = 0; // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle; //t+=0.01; @@ -119,22 +168,29 @@ void motorControl() { - //togglehoek(); button1.fall(&togglehoek); + motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s - potValue= potmeter1.read(); - //refangle= (potValue*2*pi2)-pi2; - motor1error=motor1ref-motor1angle; - - controlsignal1=PID_controller1(motor1error); - + motor1error=motor1ref-motor1angle; + controlsignal1=PID_controller1(motor1error); if (controlsignal1<0){ motordir1= 0;} else { motordir1= 1;} motor1Power.write(abs(controlsignal1)); motor1Direction= motordir1; + + motor2angle = counts2 * factorin / gearratio; // Angle of motor shaft in rad + omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s + motor2error=motor2ref-motor2angle; + controlsignal2=PID_controller2(motor2error); + if (controlsignal2<0){ + motordir2= 0;} + else { + motordir2= 1;} + motor2Power.write(abs(controlsignal2)); + motor2Direction= motordir2; } void Plotje(){ @@ -156,6 +212,7 @@ pc.printf("\r\nStarting...\r\n\r\n"); motor1Power.period(PWM_period); + motor2Power.period(PWM_period); motorTicker.attach(motorControl, 0.01); scopeTicker.attach(Plotje, 0.01); encoderTicker.attach(readEncoder, Ts); @@ -163,12 +220,11 @@ button2.fall(&toggleMotor); while (true) { - - //pc.printf("Potmeter: %d \r\n", potValue,); - //pc.printf("Counts: %i DeltaCounts: %i\r\n", counts, deltaCounts); - pc.printf("Angle: %f Omega: %f\r\n", motor1angle, omega1); - pc.printf("refangle: %f Error: %f \r\n",motor1ref, motor1error); - pc.printf("Kp: %f Ki: %f \r\n", Kp, Ki); + pc.printf("Angle1: %f Omega1: %f\r\n", motor1angle, omega1); + pc.printf("refangle1: %f Error1: %f \r\n",motor1ref, motor1error); + pc.printf("Angle2: %f Omega2: %f\r\n", motor2angle, omega2); + pc.printf("refangle2: %f Error2: %f \r\n",motor2ref, motor2error); + pc.printf("counts2: %f delta2: %f \r\n", counts2, deltaCounts2); wait(0.5); }