Code om de PID controller af te stellen aan de hand van een sinus golf
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
Revision 20:ad425c7f84b6, committed 2019-04-20
- Comitter:
- aschut
- Date:
- Sat Apr 20 19:55:10 2019 +0000
- Parent:
- 19:24f5e36c6490
- Commit message:
- Met comments
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 24f5e36c6490 -r ad425c7f84b6 main.cpp --- a/main.cpp Wed Apr 17 13:41:00 2019 +0000 +++ b/main.cpp Sat Apr 20 19:55:10 2019 +0000 @@ -3,16 +3,21 @@ #include "QEI.h" #include "BiQuad.h" #include "FastPWM.h" +// Dit script is voor het testen van motor 2, volg instructies om het om te zetten naar motor 1 + + // Algemeen Timer t; DigitalIn button2(SW2); DigitalIn button3(SW3); -DigitalIn But2(D12); +DigitalIn But2(D12); //Buttons op motorschield DigitalIn But1(D13); DigitalOut led(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); MODSERIAL pc(USBTX, USBRX); + +//waardes voor sinus signaal float A; int i = 0; float pi = 3.14159265359; @@ -107,7 +112,7 @@ { if (KPot > 0.45f) { - stap2 = KPot*150*Ts; // 144 graden van de arm in 5 seconden + stap2 = KPot*150*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } @@ -168,8 +173,8 @@ { static double error1_integral = 0; static double error1_prev = error1; // initialization with this value only done once! - static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); - /* + static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + /* Zet dit aan als je motor 1 wilt testen //PID testing Kp1 = 30 * Pot2; Ki1 = 10 * Pot1; @@ -204,8 +209,8 @@ static double error2_prev = error2; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); - // PID testing - Kp2 = 10 * Pot2; + // /*PID testing + Kp2 = 50 * Pot2; Ki2 = 10 * Pot1; if (!But2) { @@ -214,7 +219,7 @@ if (!But1){ Kd2 = Kd2 - 0.01; } - + // */Zet dit stukje uit als je motor 1 wilt testen // Proportional part: double u_k2 = Kp2 * error2; @@ -236,8 +241,8 @@ void moter1_control(double u1) { direction1= u1 > 0.0f; //positief = CW - if (fabs(u1)> 0.5f) { - u1 = 0.5f; + if (fabs(u1)> 0.99f) { + u1 = 0.99f; } else { u1= u1; } @@ -257,13 +262,13 @@ void PwmMotor(void) { - //Reference hoek berekenen, in graden - //float Ellebooghoek1 = Kinematics2(pwm2); - //float Ellebooghoek4 = Limits2(Ellebooghoek1); + //Reference hoek berekenen, in graden + //float Ellebooghoek1 = Kinematics2(pwm2); //Zet dit aan als je motor 1 wilt testen + //float Ellebooghoek4 = Limits2(Ellebooghoek1); //Zet dit aan als je motor 1 wilt testen //ElbowReference = Ellebooghoek4; - float Polshoek1 = Kinematics1(pwm2); - float Polshoek4 = Limits1(Polshoek1); + float Polshoek1 = Kinematics1(pwm2); //Zet dit uit als je motor 1 wilt testen + float Polshoek4 = Limits1(Polshoek1); //Zet dit aan als je motor 1 wilt testen //PolsReference = Polshoek4; // Positie motor berekenen, in graden @@ -305,19 +310,13 @@ } -void Kdcount (void) // Voor het testen van de PID waardes +void Kdcount (void) // Voor het testen van de PID waardes met een sinus signaal { float ts = t.read(); - A = 350; + A = 350; //Zet uit voor motor 2 ElbowReference = 350 + A*sin(2*pi*ts*0.1+(3/4)*pi); - //DA = sin(2/180*3.14)*0.5+0.5; - /* - if( i<360) { - DA = sin(i/180*3.14)*0.5+0.5; - i++; - } - */ - + //A = 900; //Zet aan voor motor 1 + //PolsReference = A*sin(2*pi*ts*0.1+(3/4)*pi); } @@ -351,7 +350,7 @@ } led = 0; - if(counter==10) { + if(counter==10) { // Dit zend waardes naar matlab, open door het script 'serialread' te gebruiken float tmp = t.read(); printf("%f,%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,error2,Kp2,Ki2,Kd2); counter = 0;