Jasper Gerth / controlandadjust

Library with P, PI and PID controller

Dependents:   buttoncontrol includeair includeair Oudverslag

controlandadjust.cpp@12:9cec66b450e0, 2015-10-09 (annotated)

Committer:
Gerth
Date:
Fri Oct 09 13:49:53 2015 +0000
Revision:
12:9cec66b450e0
Parent:
11:ef8d28e010a2
Child:
14:2b4378bfe07a
removed fault from code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:041a12a5b315 1 #include "controlandadjust.h"
Gerth 0:041a12a5b315 2 #include "mbed.h"
Gerth 0:041a12a5b315 3 #include "QEI.h"
Gerth 0:041a12a5b315 4
Gerth 7:6e2cd9b0403b 5 float CW=1;
Gerth 7:6e2cd9b0403b 6 float CCW=0;
Gerth 0:041a12a5b315 7 float direction1;
Gerth 0:041a12a5b315 8 float speed1;
Gerth 0:041a12a5b315 9 float direction2;
Gerth 0:041a12a5b315 10 float speed2;
Gerth 10:37bdb3e5f03a 11 float maxvaluepwm=0;
Gerth 0:041a12a5b315 12
Gerth 0:041a12a5b315 13 DigitalOut motor1_dir(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
Gerth 0:041a12a5b315 14 PwmOut motor1_speed(D6);//aanstuursnelheid motor 1
Gerth 0:041a12a5b315 15 PwmOut motor2_speed(D5);
Gerth 0:041a12a5b315 16 DigitalOut motor2_dir(D4);
Gerth 0:041a12a5b315 17
Gerth 0:041a12a5b315 18
Gerth 0:041a12a5b315 19
Gerth 12:9cec66b450e0 20 controlandadjust::controlandadjust(void) {}
Gerth 0:041a12a5b315 21
Gerth 0:041a12a5b315 22 void controlandadjust::verwerksignaal(float signaal1,float signaal2)
Gerth 0:041a12a5b315 23 {
Gerth 0:041a12a5b315 24 //motor1
Gerth 0:041a12a5b315 25 if (signaal1>=0) {//determine CW or CCW rotation
Gerth 0:041a12a5b315 26 direction1=CW;
Gerth 0:041a12a5b315 27 } else {
Gerth 0:041a12a5b315 28 direction1=CCW;
Gerth 0:041a12a5b315 29 }
Gerth 0:041a12a5b315 30
Gerth 10:37bdb3e5f03a 31 if (fabs(signaal1)>=maxvaluepwm) { //check if signal is <1
Gerth 10:37bdb3e5f03a 32 speed1=maxvaluepwm;//if signal >1 make it 1 to not damage motor
Gerth 0:041a12a5b315 33 } else {
Gerth 0:041a12a5b315 34 speed1=fabs(signaal1);// if signal<1 use signal
Gerth 0:041a12a5b315 35 }
Gerth 0:041a12a5b315 36
Gerth 0:041a12a5b315 37 //motor2
Gerth 0:041a12a5b315 38 if (signaal2>=0) {//determine CW or CCW rotation
Gerth 0:041a12a5b315 39 direction2=CW;
Gerth 0:041a12a5b315 40 } else {
Gerth 0:041a12a5b315 41 direction2=CCW;
Gerth 0:041a12a5b315 42 }
Gerth 0:041a12a5b315 43
Gerth 10:37bdb3e5f03a 44 if (fabs(signaal2)>=maxvaluepwm) { //check if signal is <1
Gerth 10:37bdb3e5f03a 45 speed2=maxvaluepwm;//if signal >1 make it 1 to not damage motor
Gerth 0:041a12a5b315 46 } else {
Gerth 0:041a12a5b315 47 speed2=fabs(signaal2);// if signal<1 use signal
Gerth 0:041a12a5b315 48 }
Gerth 0:041a12a5b315 49 //write to motor
Gerth 0:041a12a5b315 50 motor1_dir.write(direction1);
Gerth 0:041a12a5b315 51 motor1_speed.write(speed1);
Gerth 0:041a12a5b315 52 motor2_dir.write(direction2);
Gerth 0:041a12a5b315 53 motor2_speed.write(speed2);
Gerth 0:041a12a5b315 54
Gerth 0:041a12a5b315 55 }
Gerth 1:ece12a295ce3 56 float controlandadjust::motor1pwm()
Gerth 1:ece12a295ce3 57 {
Gerth 1:ece12a295ce3 58 return speed1;
Gerth 1:ece12a295ce3 59 }
Gerth 1:ece12a295ce3 60
Gerth 1:ece12a295ce3 61 float controlandadjust::motor2pwm()
Gerth 1:ece12a295ce3 62 {
Gerth 1:ece12a295ce3 63 return speed2;
Gerth 1:ece12a295ce3 64 }
Gerth 1:ece12a295ce3 65
Gerth 0:041a12a5b315 66
Gerth 0:041a12a5b315 67 void controlandadjust::P(float error1,float error2,float Kp)
Gerth 0:041a12a5b315 68 {
Gerth 0:041a12a5b315 69 float signaal1=error1*Kp;
Gerth 0:041a12a5b315 70 float signaal2=error2*Kp;
Gerth 0:041a12a5b315 71 verwerksignaal(signaal1,signaal2);
Gerth 0:041a12a5b315 72 }
Gerth 0:041a12a5b315 73
Gerth 0:041a12a5b315 74 void controlandadjust::PI(float error1, float error2, float Kp, float Ki,float Ts, float &error1_int, float &error2_int)
Gerth 0:041a12a5b315 75 {
Gerth 0:041a12a5b315 76 error1_int = error1_int + Ts * error1;
Gerth 0:041a12a5b315 77 float signaal1=Kp*error1+Ki*error1_int;
Gerth 0:041a12a5b315 78
Gerth 0:041a12a5b315 79 error2_int = error2_int + Ts * error2;
Gerth 0:041a12a5b315 80 float signaal2=Kp*error2+Ki*error2_int;
Gerth 0:041a12a5b315 81
Gerth 0:041a12a5b315 82 verwerksignaal (signaal1,signaal2);
Gerth 0:041a12a5b315 83 }
Gerth 0:041a12a5b315 84
Gerth 0:041a12a5b315 85 void controlandadjust::PID(float error1, float error2, float Kp, float Ki,float Kd,float Ts,
Gerth 1:ece12a295ce3 86 float &error1_int, float &error2_int, float &error1_prev, float &error2_prev)
Gerth 0:041a12a5b315 87 {
Gerth 0:041a12a5b315 88 // Derivative
Gerth 0:041a12a5b315 89 double error1_der = (error1 - error1_prev)/Ts;
Gerth 0:041a12a5b315 90 error1_prev = error1;
Gerth 0:041a12a5b315 91
Gerth 0:041a12a5b315 92 double error2_der = (error2 - error2_prev)/Ts;
Gerth 0:041a12a5b315 93 error2_prev = error2;
Gerth 0:041a12a5b315 94
Gerth 0:041a12a5b315 95 // Integral
Gerth 0:041a12a5b315 96 error1_int = error1_int + Ts * error1;
Gerth 0:041a12a5b315 97 error2_int = error2_int + Ts * error2;
Gerth 0:041a12a5b315 98 // PID
Gerth 0:041a12a5b315 99 float signaal1= Kp * error1 + Ki * error1_int + Kd * error1_der;
Gerth 0:041a12a5b315 100 float signaal2= Kp * error2 + Ki * error2_int + Kd * error2_der;
Gerth 0:041a12a5b315 101
Gerth 0:041a12a5b315 102 verwerksignaal(signaal1,signaal2);
Gerth 8:3b077fc3d1ec 103 }
Gerth 8:3b077fc3d1ec 104
Gerth 8:3b077fc3d1ec 105 void controlandadjust::STOP()
Gerth 8:3b077fc3d1ec 106 {
Gerth 9:2d57152425d1 107 motor1_speed.write(0);
Gerth 9:2d57152425d1 108 motor2_speed.write(0);
Gerth 11:ef8d28e010a2 109 }
Gerth 11:ef8d28e010a2 110 void controlandadjust::cutoff(float maxvalue)
Gerth 12:9cec66b450e0 111 {
Gerth 12:9cec66b450e0 112 maxvaluepwm=maxvalue;
Gerth 12:9cec66b450e0 113 }