Motor control
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 13:048458947701
- Parent:
- 12:23b94b5dcc60
- Child:
- 14:0afc46ad1b99
diff -r 23b94b5dcc60 -r 048458947701 main.cpp --- a/main.cpp Fri Oct 18 10:36:42 2019 +0000 +++ b/main.cpp Mon Oct 21 08:57:45 2019 +0000 @@ -15,12 +15,12 @@ AnalogIn potmeter3(A2); AnalogIn potmeter4(A3); // Encoder -DigitalIn encA(D13); -DigitalIn encB(D12); -QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING); +DigitalIn encA1(D13); +DigitalIn encB1(D12); +QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING); float Ts = 0.01; -float angle; -float omega; +float motor1angle; +float omega1; // Motor @@ -32,24 +32,22 @@ volatile int motor1Toggle = 1; //Motorcontrol -bool motordir; -double motorpwm; -double premotorpwm; -float refangle= 0; -double u2; +bool motordir1; +float motor1ref= 0; +double controlsignal1; double potValue; double pi2= 6.283185; -float e; //e = error -float Kp=0.49; -float Ki; -float u_k; -float u_i; +float motor1error; //e = error +float Kp=0.27; +float Ki=0.35; +float u_p1; +float u_i1; //Windup control -float ux; -float up; -float ek; -float ei= 0; +float ux1; +float up1; +float ek1; +float ei1= 0; float Ka= 1; //Hidscope @@ -66,56 +64,54 @@ const float PWM_period = 1e-6; -volatile int counts; // Encoder counts -volatile int countsPrev = 0; -volatile int deltaCounts; +volatile int counts1; // Encoder counts +volatile int countsPrev1 = 0; +volatile int deltaCounts1; float factorin = 6.23185/64; // Convert encoder counts to angle in rad float gearratio = 131.25; // Gear ratio of gearbox -float PID_controller(float e){ - static float error_integral=0; +float PID_controller1(float motor1error){ + static float error_integral1=0; //static float e_prev=e; //Proportional part: - Kp=potmeter1.read(); - u_k=Kp*e; + u_p1=Kp*motor1error; //Integral part - Ki=potmeter2.read(); - error_integral=error_integral+ei*Ts; - u_i=Ki*error_integral; + error_integral1=error_integral1+ei1*Ts; + u_i1=Ki*error_integral1; // Sum and limit - u2= u_k+u_i; - if (u2>1){ - up=1;} - else if (u2<-1){ - up=-1;} + up1= u_p1+u_i1; + if (up1>1){ + controlsignal1=1;} + else if (up1<-1){ + controlsignal1=-1;} else { - up=u2;} + controlsignal1=up1;} // To prevent windup - ux= u2-up; - ek= Ka*ux; - ei= e-ek; + ux1= up1-controlsignal1; + ek1= Ka*ux1; + ei1= motor1error-ek1; //Return - return up; + return controlsignal1; } void readEncoder() { - counts = encoder.getPulses(); - deltaCounts = counts - countsPrev; + counts1 = encoder1.getPulses(); + deltaCounts1 = counts1 - countsPrev1; - countsPrev = counts; + countsPrev1 = counts1; } void togglehoek(){ - refangle= 15*pi2+refangle; + motor1ref= 0.5*pi2+motor1ref; // static float t = 0; // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle; //t+=0.01; @@ -125,31 +121,26 @@ { //togglehoek(); button1.fall(&togglehoek); - angle = counts * factorin / gearratio; // Angle of motor shaft in rad - omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s + 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; - e=refangle-angle; - - u2=PID_controller(e); + motor1error=motor1ref-motor1angle; - premotorpwm= fabs(u2); - if (premotorpwm>1.0){ - motorpwm=1;} + controlsignal1=PID_controller1(motor1error); + + if (controlsignal1<0){ + motordir1= 0;} else { - motorpwm=premotorpwm;} - if (u2<0){ - motordir= 0;} - else { - motordir= 1;} - motor1Power.write(motorpwm); - motor1Direction= motordir; + motordir1= 1;} + motor1Power.write(abs(controlsignal1)); + motor1Direction= motordir1; } void Plotje(){ - scope.set(0,refangle); //gewenste hoek - scope.set(1,angle); //Gemeten hoek - scope.set(2,e); //verschil in gewenste en gemeten hoek + scope.set(0,motor1ref); //gewenste hoek + scope.set(1,motor1angle); //Gemeten hoek + scope.set(2,motor1error); //verschil in gewenste en gemeten hoek scope.send(); //send what's in scope memory to PC } @@ -175,8 +166,8 @@ //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", angle, omega); - pc.printf("refangle: %f Error: %f \r\n",refangle, e); + 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); wait(0.5);