![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
Diff: main.cpp
- Revision:
- 24:b9dd6cf5c366
- Parent:
- 23:c027e5c57cc8
- Child:
- 25:c719346df3cd
diff -r c027e5c57cc8 -r b9dd6cf5c366 main.cpp --- a/main.cpp Tue Oct 30 09:37:29 2018 +0000 +++ b/main.cpp Tue Oct 30 15:02:26 2018 +0000 @@ -1,5 +1,6 @@ #include "mbed.h" #include "MODSERIAL.h" +#include "BiQuad.h" AnalogIn potmetervalue1 (A1); AnalogIn potmetervalue2 (A2); @@ -19,7 +20,17 @@ //Global variables -int encoder = 0; +int encoder = 0; //Start value encoder +int T = 0.002f; //Ticker period + +double Kp = 17.5; +double Ki = 1.02; +double Kd = 23.2; + +double err = 0; + +//Tickers +Ticker function_tick; //Functions @@ -71,6 +82,43 @@ } } +void encoder_count() +{ + encoderA.rise(&encoderA_rise); + encoderA.fall(&encoderA_fall); + encoderB.rise(&encoderB_rise); + encoderB.fall(&encoderB_fall); +} + +double PID_controller() +{ + static double err_integral = 0; + static double err_prev = err; // initialization with this value only done once! + + static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + // Proportional part: + double u_k = Kp * err; + + // Integral part + err_integral = err_integral + err * T; + double u_i = Ki * err_integral; + + // Derivative part + double err_derivative = (err - err_prev)/T; + double filtered_err_derivative = LowPassFilter.step(err_derivative); + double u_d = Kd * filtered_err_derivative; + err_prev = err; + + // Sum all parts and return it + return u_k + u_i + u_d; +} + +void call_all_functions() +{ + PID_controller(); + encoder_count(); +} // Main function start. @@ -80,27 +128,14 @@ pc.printf("hello\n\r"); pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz - encoderA.rise(&encoderA_rise); - encoderA.fall(&encoderA_fall); - encoderB.rise(&encoderB_rise); - encoderB.fall(&encoderB_fall); + function_tick.attach(&call_all_functions,T); + while (true) { - - float u1 = potmetervalue1; - float u2 = potmetervalue2; - - float m1 = ((u1*2.0f)-1.0f); - float m2 = ((u2*2.0f)-1.0f); - pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling. - directionpin1.write(m1>0); //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. - wait(0.01f); //zodat de code niet oneindig doorgaat. - pwmpin2 = fabs(m2*0.6f)+0.4f; - directionpin2.write(m2>0); + - pc.printf("Encoder count: %i \n\r",encoder); //We moeten de encoder counts nog omzetten naar radialen of graden? } }