Library with P, PI and PID controller
Dependents: buttoncontrol includeair includeair Oudverslag
controlandadjust.cpp
- Committer:
- Gerth
- Date:
- 2015-10-08
- Revision:
- 6:9addcf4d7af3
- Parent:
- 5:5794dbc42c3d
- Child:
- 7:6e2cd9b0403b
File content as of revision 6:9addcf4d7af3:
#include "controlandadjust.h" #include "mbed.h" #include "QEI.h" float CW=0; float CCW=1; float direction1; float speed1; float direction2; float speed2; DigitalOut motor1_dir(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt) PwmOut motor1_speed(D6);//aanstuursnelheid motor 1 PwmOut motor2_speed(D5); DigitalOut motor2_dir(D4); controlandadjust::controlandadjust(void) { //motor1_speed.write(0); // motor2_speed.write(0); } void controlandadjust::verwerksignaal(float signaal1,float signaal2) { //motor1 if (signaal1>=0) {//determine CW or CCW rotation direction1=CW; } else { direction1=CCW; } if (fabs(signaal1)>=1) { //check if signal is <1 speed1=1;//if signal >1 make it 1 to not damage motor } else { speed1=fabs(signaal1);// if signal<1 use signal } //motor2 if (signaal2>=0) {//determine CW or CCW rotation direction2=CW; } else { direction2=CCW; } if (fabs(signaal2)>=1) { //check if signal is <1 speed2=1;//if signal >1 make it 1 to not damage motor } else { speed2=fabs(signaal2);// if signal<1 use signal } //write to motor motor1_dir.write(direction1); motor1_speed.write(speed1); motor2_dir.write(direction2); motor2_speed.write(speed2); } float controlandadjust::motor1pwm() { return speed1; } float controlandadjust::motor2pwm() { return speed2; } void controlandadjust::P(float error1,float error2,float Kp) { float signaal1=error1*Kp; float signaal2=error2*Kp; verwerksignaal(signaal1,signaal2); } void controlandadjust::PI(float error1, float error2, float Kp, float Ki,float Ts, float &error1_int, float &error2_int) { error1_int = error1_int + Ts * error1; float signaal1=Kp*error1+Ki*error1_int; error2_int = error2_int + Ts * error2; float signaal2=Kp*error2+Ki*error2_int; verwerksignaal (signaal1,signaal2); } void controlandadjust::PID(float error1, float error2, float Kp, float Ki,float Kd,float Ts, float &error1_int, float &error2_int, float &error1_prev, float &error2_prev) { // Derivative double error1_der = (error1 - error1_prev)/Ts; error1_prev = error1; double error2_der = (error2 - error2_prev)/Ts; error2_prev = error2; // Integral error1_int = error1_int + Ts * error1; error2_int = error2_int + Ts * error2; // PID float signaal1= Kp * error1 + Ki * error1_int + Kd * error1_der; float signaal2= Kp * error2 + Ki * error2_int + Kd * error2_der; verwerksignaal(signaal1,signaal2); }