Jasper Gerth / controlandadjust

Library with P, PI and PID controller

Dependents:   buttoncontrol includeair includeair Oudverslag

controlandadjust.cpp@16:57c4f4ac11e4, 2015-10-23 (annotated)

Committer:
Gerth
Date:
Fri Oct 23 12:13:00 2015 +0000
Revision:
16:57c4f4ac11e4
Parent:
15:a1a29db96f4f
Child:
17:666505754e3f
made error_int zero if the error is whitin the allowed error to prevent controller from spooling up if pod stays stationairy.

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 15:a1a29db96f4f 12 float minimal_error;
Gerth 15:a1a29db96f4f 13 const float pi=3.14159265359;
Gerth 15:a1a29db96f4f 14 const float degtorad=(pi/180);
Gerth 0:041a12a5b315 15
Gerth 16:57c4f4ac11e4 16
Gerth 16:57c4f4ac11e4 17 //
Gerth 16:57c4f4ac11e4 18 Serial pcshit(USBTX,USBRX);
Gerth 16:57c4f4ac11e4 19
Gerth 0:041a12a5b315 20 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 21 PwmOut motor1_speed(D6);//aanstuursnelheid motor 1
Gerth 0:041a12a5b315 22 PwmOut motor2_speed(D5);
Gerth 0:041a12a5b315 23 DigitalOut motor2_dir(D4);
Gerth 0:041a12a5b315 24
Gerth 0:041a12a5b315 25
Gerth 0:041a12a5b315 26
Gerth 15:a1a29db96f4f 27 controlandadjust::controlandadjust(float errorband)
Gerth 15:a1a29db96f4f 28 {
Gerth 15:a1a29db96f4f 29 minimal_error=errorband*degtorad/2;
Gerth 15:a1a29db96f4f 30 }
Gerth 0:041a12a5b315 31
Gerth 0:041a12a5b315 32 void controlandadjust::verwerksignaal(float signaal1,float signaal2)
Gerth 0:041a12a5b315 33 {
Gerth 0:041a12a5b315 34 //motor1
Gerth 0:041a12a5b315 35 if (signaal1>=0) {//determine CW or CCW rotation
Gerth 14:2b4378bfe07a 36 direction1=CCW;
Gerth 0:041a12a5b315 37 } else {
Gerth 14:2b4378bfe07a 38 direction1=CW;
Gerth 0:041a12a5b315 39 }
Gerth 0:041a12a5b315 40
Gerth 10:37bdb3e5f03a 41 if (fabs(signaal1)>=maxvaluepwm) { //check if signal is <1
Gerth 10:37bdb3e5f03a 42 speed1=maxvaluepwm;//if signal >1 make it 1 to not damage motor
Gerth 0:041a12a5b315 43 } else {
Gerth 0:041a12a5b315 44 speed1=fabs(signaal1);// if signal<1 use signal
Gerth 0:041a12a5b315 45 }
Gerth 0:041a12a5b315 46
Gerth 0:041a12a5b315 47 //motor2
Gerth 0:041a12a5b315 48 if (signaal2>=0) {//determine CW or CCW rotation
Gerth 0:041a12a5b315 49 direction2=CW;
Gerth 0:041a12a5b315 50 } else {
Gerth 0:041a12a5b315 51 direction2=CCW;
Gerth 0:041a12a5b315 52 }
Gerth 0:041a12a5b315 53
Gerth 10:37bdb3e5f03a 54 if (fabs(signaal2)>=maxvaluepwm) { //check if signal is <1
Gerth 10:37bdb3e5f03a 55 speed2=maxvaluepwm;//if signal >1 make it 1 to not damage motor
Gerth 0:041a12a5b315 56 } else {
Gerth 0:041a12a5b315 57 speed2=fabs(signaal2);// if signal<1 use signal
Gerth 0:041a12a5b315 58 }
Gerth 0:041a12a5b315 59 //write to motor
Gerth 0:041a12a5b315 60 motor1_dir.write(direction1);
Gerth 0:041a12a5b315 61 motor1_speed.write(speed1);
Gerth 0:041a12a5b315 62 motor2_dir.write(direction2);
Gerth 0:041a12a5b315 63 motor2_speed.write(speed2);
Gerth 0:041a12a5b315 64
Gerth 0:041a12a5b315 65 }
Gerth 1:ece12a295ce3 66 float controlandadjust::motor1pwm()
Gerth 1:ece12a295ce3 67 {
Gerth 1:ece12a295ce3 68 return speed1;
Gerth 1:ece12a295ce3 69 }
Gerth 1:ece12a295ce3 70
Gerth 1:ece12a295ce3 71 float controlandadjust::motor2pwm()
Gerth 1:ece12a295ce3 72 {
Gerth 1:ece12a295ce3 73 return speed2;
Gerth 1:ece12a295ce3 74 }
Gerth 1:ece12a295ce3 75
Gerth 0:041a12a5b315 76
Gerth 0:041a12a5b315 77 void controlandadjust::P(float error1,float error2,float Kp)
Gerth 0:041a12a5b315 78 {
Gerth 0:041a12a5b315 79 float signaal1=error1*Kp;
Gerth 0:041a12a5b315 80 float signaal2=error2*Kp;
Gerth 15:a1a29db96f4f 81
Gerth 15:a1a29db96f4f 82 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 15:a1a29db96f4f 83 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 15:a1a29db96f4f 84 verwerksignaal(signaal1,signaal2);
Gerth 15:a1a29db96f4f 85 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 15:a1a29db96f4f 86 verwerksignaal(0,0);
Gerth 15:a1a29db96f4f 87 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 15:a1a29db96f4f 88 verwerksignaal(signaal1,0);
Gerth 15:a1a29db96f4f 89 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 15:a1a29db96f4f 90 verwerksignaal(0,signaal2);
Gerth 15:a1a29db96f4f 91 }
Gerth 0:041a12a5b315 92 }
Gerth 0:041a12a5b315 93
Gerth 0:041a12a5b315 94 void controlandadjust::PI(float error1, float error2, float Kp, float Ki,float Ts, float &error1_int, float &error2_int)
Gerth 0:041a12a5b315 95 {
Gerth 0:041a12a5b315 96 error1_int = error1_int + Ts * error1;
Gerth 0:041a12a5b315 97 float signaal1=Kp*error1+Ki*error1_int;
Gerth 0:041a12a5b315 98
Gerth 0:041a12a5b315 99 error2_int = error2_int + Ts * error2;
Gerth 0:041a12a5b315 100 float signaal2=Kp*error2+Ki*error2_int;
Gerth 0:041a12a5b315 101
Gerth 15:a1a29db96f4f 102 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 16:57c4f4ac11e4 103 //and make error_int 0 if the error is small enough to protect from "spooling up"
Gerth 15:a1a29db96f4f 104 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 15:a1a29db96f4f 105 verwerksignaal(signaal1,signaal2);
Gerth 15:a1a29db96f4f 106 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 15:a1a29db96f4f 107 verwerksignaal(0,0);
Gerth 16:57c4f4ac11e4 108 error1_int=0,error2_int=0;
Gerth 15:a1a29db96f4f 109 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 15:a1a29db96f4f 110 verwerksignaal(signaal1,0);
Gerth 16:57c4f4ac11e4 111 error2_int=0;
Gerth 15:a1a29db96f4f 112 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 15:a1a29db96f4f 113 verwerksignaal(0,signaal2);
Gerth 16:57c4f4ac11e4 114 error1_int=0;
Gerth 15:a1a29db96f4f 115 }
Gerth 0:041a12a5b315 116 }
Gerth 0:041a12a5b315 117
Gerth 0:041a12a5b315 118 void controlandadjust::PID(float error1, float error2, float Kp, float Ki,float Kd,float Ts,
Gerth 1:ece12a295ce3 119 float &error1_int, float &error2_int, float &error1_prev, float &error2_prev)
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 0:041a12a5b315 132 float signaal1= Kp * error1 + Ki * error1_int + Kd * error1_der;
Gerth 0:041a12a5b315 133 float signaal2= Kp * error2 + Ki * error2_int + Kd * error2_der;
Gerth 0:041a12a5b315 134
Gerth 15:a1a29db96f4f 135 //check if error is big enough to produce signal, else signal is 0 to save motors
Gerth 16:57c4f4ac11e4 136 //and make error_int 0 if the error is small enough to protect from "spooling up"
Gerth 15:a1a29db96f4f 137 if (fabs(error1) > minimal_error && fabs(error2) > minimal_error) {
Gerth 15:a1a29db96f4f 138 verwerksignaal(signaal1,signaal2);
Gerth 15:a1a29db96f4f 139 } else if (fabs(error1) <= minimal_error && fabs(error2) <= minimal_error) {
Gerth 15:a1a29db96f4f 140 verwerksignaal(0,0);
Gerth 16:57c4f4ac11e4 141 error1_int=0,error2_int=0;
Gerth 15:a1a29db96f4f 142 } else if (fabs(error1) > minimal_error && fabs(error2) <= minimal_error) {
Gerth 15:a1a29db96f4f 143 verwerksignaal(signaal1,0);
Gerth 16:57c4f4ac11e4 144 error2_int=0;
Gerth 15:a1a29db96f4f 145 } else if (fabs(error1) <= minimal_error && fabs(error2) > minimal_error) {
Gerth 15:a1a29db96f4f 146 verwerksignaal(0,signaal2);
Gerth 16:57c4f4ac11e4 147 error1_int=0;
Gerth 15:a1a29db96f4f 148 }
Gerth 8:3b077fc3d1ec 149 }
Gerth 8:3b077fc3d1ec 150
Gerth 8:3b077fc3d1ec 151 void controlandadjust::STOP()
Gerth 8:3b077fc3d1ec 152 {
Gerth 9:2d57152425d1 153 motor1_speed.write(0);
Gerth 9:2d57152425d1 154 motor2_speed.write(0);
Gerth 11:ef8d28e010a2 155 }
Gerth 11:ef8d28e010a2 156 void controlandadjust::cutoff(float maxvalue)
Gerth 12:9cec66b450e0 157 {
Gerth 15:a1a29db96f4f 158 maxvaluepwm=maxvalue;
Gerth 12:9cec66b450e0 159 }