Jasper Gerth / controlandadjust

Library with P, PI and PID controller

Dependents:   buttoncontrol includeair includeair Oudverslag

controlandadjust.cpp@19:e3585d3c5a85, 2015-10-29 (annotated)

Committer:
Gerth
Date:
Thu Oct 29 14:02:20 2015 +0000
Revision:
19:e3585d3c5a85
Parent:
18:ef413d2fd0b1
Child:
20:a9a6ffb32dbf
converted floats to doubles

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 19:e3585d3c5a85 4 //constants
Gerth 19:e3585d3c5a85 5 int CW=1;
Gerth 19:e3585d3c5a85 6 int CCW=0;
Gerth 19:e3585d3c5a85 7 int direction1,direction2;
Gerth 19:e3585d3c5a85 8 double speed1,speed2;
Gerth 19:e3585d3c5a85 9 double signal1,signal2;
Gerth 19:e3585d3c5a85 10 double error1_int,error2_int;
Gerth 19:e3585d3c5a85 11 double error1_prev,error2_prev;
Gerth 17:666505754e3f 12
Gerth 10:37bdb3e5f03a 13 float maxvaluepwm=0;
Gerth 15:a1a29db96f4f 14 float minimal_error;
Gerth 19:e3585d3c5a85 15 double Ts;
Gerth 19:e3585d3c5a85 16 const double pi=3.14159265359;
Gerth 19:e3585d3c5a85 17 const double degtorad=(pi/180);
Gerth 0:041a12a5b315 18
Gerth 19:e3585d3c5a85 19 //Outputs
Gerth 19:e3585d3c5a85 20 DigitalOut motor1_dir(D7);// rotation directoin motor 1, (1 is CW when looking to the top of the encoder(or shaft))
Gerth 19:e3585d3c5a85 21 PwmOut motor1_speed(D6);//speed motor 1
Gerth 0:041a12a5b315 22 PwmOut motor2_speed(D5);
Gerth 0:041a12a5b315 23 DigitalOut motor2_dir(D4);
Gerth 0:041a12a5b315 24
Gerth 19:e3585d3c5a85 25 //constuctor
Gerth 19:e3585d3c5a85 26 controlandadjust::controlandadjust(float errorband, double controlfrequency)
Gerth 15:a1a29db96f4f 27 {
Gerth 15:a1a29db96f4f 28 minimal_error=errorband*degtorad/2;
Gerth 17:666505754e3f 29 Ts=1.0/controlfrequency;
Gerth 15:a1a29db96f4f 30 }
Gerth 0:041a12a5b315 31
Gerth 19:e3585d3c5a85 32 //take signal and write the rigth values to motors
Gerth 19:e3585d3c5a85 33 void controlandadjust::process_signal(double signal1,double signal2)
Gerth 0:041a12a5b315 34 {
Gerth 0:041a12a5b315 35 //motor1
Gerth 19:e3585d3c5a85 36 if (signal1>=0) {//determine CW or CCW rotation
Gerth 14:2b4378bfe07a 37 direction1=CCW;
Gerth 0:041a12a5b315 38 } else {
Gerth 14:2b4378bfe07a 39 direction1=CW;
Gerth 0:041a12a5b315 40 }
Gerth 0:041a12a5b315 41
Gerth 19:e3585d3c5a85 42 if (fabs(signal1)>=maxvaluepwm) { //check if signal is <1
Gerth 10:37bdb3e5f03a 43 speed1=maxvaluepwm;//if signal >1 make it 1 to not damage motor
Gerth 0:041a12a5b315 44 } else {
Gerth 19:e3585d3c5a85 45 speed1=fabs(signal1);// if signal<1 use signal
Gerth 0:041a12a5b315 46 }
Gerth 0:041a12a5b315 47
Gerth 0:041a12a5b315 48 //motor2
Gerth 19:e3585d3c5a85 49 if (signal2>=0) {//determine CW or CCW rotation
Gerth 0:041a12a5b315 50 direction2=CW;
Gerth 0:041a12a5b315 51 } else {
Gerth 0:041a12a5b315 52 direction2=CCW;
Gerth 0:041a12a5b315 53 }
Gerth 0:041a12a5b315 54
Gerth 19:e3585d3c5a85 55 if (fabs(signal2)>=maxvaluepwm) { //check if signal is <1
Gerth 10:37bdb3e5f03a 56 speed2=maxvaluepwm;//if signal >1 make it 1 to not damage motor
Gerth 0:041a12a5b315 57 } else {
Gerth 19:e3585d3c5a85 58 speed2=fabs(signal2);// if signal<1 use signal
Gerth 0:041a12a5b315 59 }
Gerth 0:041a12a5b315 60 //write to motor
Gerth 0:041a12a5b315 61 motor1_dir.write(direction1);
Gerth 0:041a12a5b315 62 motor1_speed.write(speed1);
Gerth 0:041a12a5b315 63 motor2_dir.write(direction2);
Gerth 0:041a12a5b315 64 motor2_speed.write(speed2);
Gerth 0:041a12a5b315 65
Gerth 0:041a12a5b315 66 }
Gerth 1:ece12a295ce3 67 float controlandadjust::motor1pwm()
Gerth 1:ece12a295ce3 68 {
Gerth 1:ece12a295ce3 69 return speed1;
Gerth 1:ece12a295ce3 70 }
Gerth 1:ece12a295ce3 71
Gerth 1:ece12a295ce3 72 float controlandadjust::motor2pwm()
Gerth 1:ece12a295ce3 73 {
Gerth 1:ece12a295ce3 74 return speed2;
Gerth 1:ece12a295ce3 75 }
Gerth 1:ece12a295ce3 76
Gerth 0:041a12a5b315 77
Gerth 19:e3585d3c5a85 78 void controlandadjust::P(double error1,double error2,float Kp)
Gerth 0:041a12a5b315 79 {
Gerth 19:e3585d3c5a85 80 signal1=error1*Kp;
Gerth 19:e3585d3c5a85 81 signal2=error2*Kp;
Gerth 17:666505754e3f 82
Gerth 15:a1a29db96f4f 83 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 15:a1a29db96f4f 84 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 85 process_signal(signal1,signal2);
Gerth 15:a1a29db96f4f 86 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 87 process_signal(0,0);
Gerth 15:a1a29db96f4f 88 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 89 process_signal(signal1,0);
Gerth 15:a1a29db96f4f 90 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 91 process_signal(0,signal2);
Gerth 15:a1a29db96f4f 92 }
Gerth 0:041a12a5b315 93 }
Gerth 0:041a12a5b315 94
Gerth 19:e3585d3c5a85 95 void controlandadjust::PI(double error1, double error2, float Kp, float Ki)
Gerth 0:041a12a5b315 96 {
Gerth 0:041a12a5b315 97 error1_int = error1_int + Ts * error1;
Gerth 19:e3585d3c5a85 98 signal1=Kp*error1+Ki*error1_int;
Gerth 0:041a12a5b315 99
Gerth 0:041a12a5b315 100 error2_int = error2_int + Ts * error2;
Gerth 19:e3585d3c5a85 101 signal2=Kp*error2+Ki*error2_int;
Gerth 0:041a12a5b315 102
Gerth 17:666505754e3f 103 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 17:666505754e3f 104 //and make error_int 0 if the error is small enough to protect from "spooling up"
Gerth 15:a1a29db96f4f 105 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 106 process_signal(signal1,signal2);
Gerth 15:a1a29db96f4f 107 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 108 process_signal(0,0);
Gerth 16:57c4f4ac11e4 109 error1_int=0,error2_int=0;
Gerth 15:a1a29db96f4f 110 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 111 process_signal(signal1,0);
Gerth 16:57c4f4ac11e4 112 error2_int=0;
Gerth 15:a1a29db96f4f 113 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 114 process_signal(0,signal2);
Gerth 16:57c4f4ac11e4 115 error1_int=0;
Gerth 15:a1a29db96f4f 116 }
Gerth 0:041a12a5b315 117 }
Gerth 0:041a12a5b315 118
Gerth 19:e3585d3c5a85 119 void controlandadjust::PID(double error1, double error2, float Kp, float Ki,float Kd)
Gerth 0:041a12a5b315 120 {
Gerth 0:041a12a5b315 121 // Derivative
Gerth 0:041a12a5b315 122 double error1_der = (error1 - error1_prev)/Ts;
Gerth 0:041a12a5b315 123 error1_prev = error1;
Gerth 0:041a12a5b315 124
Gerth 0:041a12a5b315 125 double error2_der = (error2 - error2_prev)/Ts;
Gerth 0:041a12a5b315 126 error2_prev = error2;
Gerth 0:041a12a5b315 127
Gerth 0:041a12a5b315 128 // Integral
Gerth 0:041a12a5b315 129 error1_int = error1_int + Ts * error1;
Gerth 0:041a12a5b315 130 error2_int = error2_int + Ts * error2;
Gerth 0:041a12a5b315 131 // PID
Gerth 19:e3585d3c5a85 132 signal1= Kp * error1 + Ki * error1_int + Kd * error1_der;
Gerth 19:e3585d3c5a85 133 signal2= Kp * error2 + Ki * error2_int + Kd * error2_der;
Gerth 0:041a12a5b315 134
Gerth 17:666505754e3f 135 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 17:666505754e3f 136 //and make error_int 0 if the error is small enough to protect from "spooling up"
Gerth 17:666505754e3f 137 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 138 process_signal(signal1,signal2);
Gerth 17:666505754e3f 139 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 140 process_signal(0,0);
Gerth 17:666505754e3f 141 error1_int=0,error2_int=0;
Gerth 17:666505754e3f 142 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 143 process_signal(signal1,0);
Gerth 17:666505754e3f 144 error2_int=0;
Gerth 17:666505754e3f 145 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 146 process_signal(0,signal2);
Gerth 17:666505754e3f 147 error1_int=0;
Gerth 17:666505754e3f 148 }
Gerth 17:666505754e3f 149 }
Gerth 17:666505754e3f 150 //////////////////////////////////////PID WITH LOW PASS FILTER
Gerth 19:e3585d3c5a85 151 void controlandadjust::PIDLowPass(double error1, double error2, float Kp, float Ki,float Kd, double tau_p)
Gerth 17:666505754e3f 152 {
Gerth 17:666505754e3f 153 // Derivative
Gerth 17:666505754e3f 154 double error1_der = (error1 - error1_prev)/Ts; //(e(k)-e(k-1))/Ts
Gerth 17:666505754e3f 155 error1_prev = error1;
Gerth 17:666505754e3f 156
Gerth 17:666505754e3f 157 double error2_der = (error2 - error2_prev)/Ts;
Gerth 17:666505754e3f 158 error2_prev = error2;
Gerth 17:666505754e3f 159
Gerth 17:666505754e3f 160 // Integral
Gerth 17:666505754e3f 161 error1_int = error1_int + Ts * error1;
Gerth 17:666505754e3f 162 error2_int = error2_int + Ts * error2;
Gerth 17:666505754e3f 163 // PID
Gerth 19:e3585d3c5a85 164 signal1= (signal1*(tau_p/Ts) + Kp * error1 + Ki * error1_int + Kd * error1_der)*( 1/( 1+ (tau_p/Ts) ) );
Gerth 19:e3585d3c5a85 165 signal2= (signal2*(tau_p/Ts) + Kp * error2 + Ki * error2_int + Kd * error2_der)*( 1/( 1+ (tau_p/Ts) ) );
Gerth 17:666505754e3f 166
Gerth 17:666505754e3f 167 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 17:666505754e3f 168 //and make error_int 0 if the error is small enough to protect from "spooling up"
Gerth 15:a1a29db96f4f 169 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 170 process_signal(signal1,signal2);
Gerth 15:a1a29db96f4f 171 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 172 process_signal(0,0);
Gerth 16:57c4f4ac11e4 173 error1_int=0,error2_int=0;
Gerth 15:a1a29db96f4f 174 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 19:e3585d3c5a85 175 process_signal(signal1,0);
Gerth 16:57c4f4ac11e4 176 error2_int=0;
Gerth 15:a1a29db96f4f 177 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 19:e3585d3c5a85 178 process_signal(0,signal2);
Gerth 16:57c4f4ac11e4 179 error1_int=0;
Gerth 15:a1a29db96f4f 180 }
Gerth 8:3b077fc3d1ec 181 }
Gerth 8:3b077fc3d1ec 182
Gerth 8:3b077fc3d1ec 183 void controlandadjust::STOP()
Gerth 8:3b077fc3d1ec 184 {
Gerth 9:2d57152425d1 185 motor1_speed.write(0);
Gerth 9:2d57152425d1 186 motor2_speed.write(0);
Gerth 11:ef8d28e010a2 187 }
Gerth 11:ef8d28e010a2 188 void controlandadjust::cutoff(float maxvalue)
Gerth 12:9cec66b450e0 189 {
Gerth 15:a1a29db96f4f 190 maxvaluepwm=maxvalue;
Gerth 12:9cec66b450e0 191 }