BMT K9 groep 9 2015-2016 / Mbed 2 deprecated frdm_PIDControllertest

Goed werkende PID, vanaf nu dit script gebruiken.

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@3:eb5b57162e38, 2015-10-23 (annotated)

Committer:
roosalyn
Date:
Fri Oct 23 07:36:45 2015 +0000
Revision:
3:eb5b57162e38
Parent:
0:50d492ea0fd0
Child:
4:6d88a281192f
om de constantes te bepalen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roosalyn 0:50d492ea0fd0 1 #include "mbed.h"
roosalyn 0:50d492ea0fd0 2 #include "encoder.h"
roosalyn 0:50d492ea0fd0 3 #include "MODSERIAL.h"
roosalyn 0:50d492ea0fd0 4 #include "math.h"
roosalyn 0:50d492ea0fd0 5 #include "HIDScope.h"
roosalyn 0:50d492ea0fd0 6 #define M_PI 3.14159265358979323846
roosalyn 0:50d492ea0fd0 7
roosalyn 0:50d492ea0fd0 8 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
roosalyn 0:50d492ea0fd0 9 PwmOut motor2speed(D5);
roosalyn 0:50d492ea0fd0 10 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
roosalyn 0:50d492ea0fd0 11 PwmOut motor1speed(D6);
roosalyn 0:50d492ea0fd0 12 AnalogIn potmeter1(A3); //blauw draaiknopje, nr1
roosalyn 0:50d492ea0fd0 13 AnalogIn potmeter2(A2); //nr 2
roosalyn 0:50d492ea0fd0 14 Encoder encoder2(D13,D12,true);
roosalyn 0:50d492ea0fd0 15 Encoder encoder1(D11,D10,true);
roosalyn 0:50d492ea0fd0 16 MODSERIAL pc(USBTX,USBRX);
roosalyn 0:50d492ea0fd0 17
roosalyn 0:50d492ea0fd0 18 Ticker myControllerTicker1, myControllerTicker2, countStepTickerX, countStepTickerY, buttonStateCheckX, buttonStateCheckY;
roosalyn 0:50d492ea0fd0 19 bool motor1_PID_Controller_go = false;
roosalyn 0:50d492ea0fd0 20 bool countStepX_go = false;
roosalyn 0:50d492ea0fd0 21 bool countStepY_go = false;
roosalyn 0:50d492ea0fd0 22 bool checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 23 bool checkButtonStateY_go = false;
roosalyn 0:50d492ea0fd0 24
roosalyn 0:50d492ea0fd0 25 DigitalIn telknopX(SW2);
roosalyn 0:50d492ea0fd0 26 DigitalIn telknopY(SW3);
roosalyn 0:50d492ea0fd0 27 DigitalOut ledX(D2);
roosalyn 0:50d492ea0fd0 28 DigitalOut ledY(D3);
roosalyn 0:50d492ea0fd0 29 HIDScope scope(2);
roosalyn 0:50d492ea0fd0 30
roosalyn 0:50d492ea0fd0 31 const double motor1_Kp = 1; //4200=1 rondje
roosalyn 0:50d492ea0fd0 32 double m1_Kp = 0.0005, m1_Ki = 0.0, m1_Kd = 0.0;
roosalyn 0:50d492ea0fd0 33 const double m1_Ts = 0.01;
roosalyn 0:50d492ea0fd0 34 double m1_err_int = 0, m1_prev_err = 0;
roosalyn 0:50d492ea0fd0 35 double m1_u_prev = 0;
roosalyn 0:50d492ea0fd0 36
roosalyn 0:50d492ea0fd0 37 const int grens = 50;
roosalyn 0:50d492ea0fd0 38
roosalyn 0:50d492ea0fd0 39 /*
roosalyn 0:50d492ea0fd0 40 Gain = 0.002356
roosalyn 0:50d492ea0fd0 41 B = [ 1.00000000000, 2.00000000000, 1.00000000000]
roosalyn 0:50d492ea0fd0 42 A = [ 1.00000000000, -1.85808607200, 0.86750940236]
roosalyn 0:50d492ea0fd0 43 */
roosalyn 0:50d492ea0fd0 44
roosalyn 0:50d492ea0fd0 45 const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431;
roosalyn 0:50d492ea0fd0 46 //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
roosalyn 0:50d492ea0fd0 47 double m1_f_v1 = 0, m1_f_v2 = 0; //de variabelen van de filter
roosalyn 0:50d492ea0fd0 48
roosalyn 0:50d492ea0fd0 49 int reference1;
roosalyn 0:50d492ea0fd0 50 int reference2;
roosalyn 0:50d492ea0fd0 51 int difference1;
roosalyn 0:50d492ea0fd0 52 int difference2;
roosalyn 0:50d492ea0fd0 53 int buttonStateX = 1;
roosalyn 0:50d492ea0fd0 54 int buttonStateY = 1;
roosalyn 0:50d492ea0fd0 55 int motorDirectionX = 1;
roosalyn 0:50d492ea0fd0 56 int motorDirectionY = 1;
roosalyn 0:50d492ea0fd0 57 int n;
roosalyn 0:50d492ea0fd0 58 int countsX=0;
roosalyn 0:50d492ea0fd0 59 int countsY=0;
roosalyn 0:50d492ea0fd0 60 int countStateX = 1;
roosalyn 0:50d492ea0fd0 61 int countStateY = 1;
roosalyn 0:50d492ea0fd0 62
roosalyn 0:50d492ea0fd0 63 float x = 4.0; // voorbeeld, moet eigenlijk bepaald worden door EMG signaal
roosalyn 0:50d492ea0fd0 64 float y = 4.0; // is het 4de vakje van rechtsonderin de hoek geteld
roosalyn 0:50d492ea0fd0 65 float xdist_from_board = 4.0; //er moet 2 van afgetrokken worden. Als de as 6cm van het bord staat moet er 4 worden ingevuld. Telt namelijk
roosalyn 0:50d492ea0fd0 66 float ydist_from_board = 4.0; //vanaf het midden van een vakje.
roosalyn 0:50d492ea0fd0 67 double constante1;
roosalyn 0:50d492ea0fd0 68 double constante2;
roosalyn 0:50d492ea0fd0 69 double constante3;
roosalyn 0:50d492ea0fd0 70
roosalyn 0:50d492ea0fd0 71 bool buttonReleasedX(DigitalIn& button)
roosalyn 0:50d492ea0fd0 72 {
roosalyn 0:50d492ea0fd0 73 if (button.read() == 1 && buttonStateX == 0) {
roosalyn 0:50d492ea0fd0 74 buttonStateX = 1;
roosalyn 0:50d492ea0fd0 75 return true;
roosalyn 0:50d492ea0fd0 76 }
roosalyn 0:50d492ea0fd0 77 buttonStateX = button.read();
roosalyn 0:50d492ea0fd0 78 return false;
roosalyn 0:50d492ea0fd0 79 }
roosalyn 0:50d492ea0fd0 80
roosalyn 0:50d492ea0fd0 81 bool buttonReleasedY(DigitalIn& button)
roosalyn 0:50d492ea0fd0 82 {
roosalyn 0:50d492ea0fd0 83 if (button.read() == 1 && buttonStateY == 0) {
roosalyn 0:50d492ea0fd0 84 buttonStateY = 1;
roosalyn 0:50d492ea0fd0 85 return true;
roosalyn 0:50d492ea0fd0 86 }
roosalyn 0:50d492ea0fd0 87 buttonStateY = button.read();
roosalyn 0:50d492ea0fd0 88 return false;
roosalyn 0:50d492ea0fd0 89 }
roosalyn 0:50d492ea0fd0 90
roosalyn 0:50d492ea0fd0 91 bool buttonHold(DigitalIn& button)
roosalyn 0:50d492ea0fd0 92 {
roosalyn 0:50d492ea0fd0 93 return (button.read() == 0);
roosalyn 0:50d492ea0fd0 94 }
roosalyn 0:50d492ea0fd0 95
roosalyn 0:50d492ea0fd0 96 float bereken_hoek1(float x, float y)
roosalyn 0:50d492ea0fd0 97 {
roosalyn 0:50d492ea0fd0 98 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 0:50d492ea0fd0 99 float alpha = acos((langste_zijde/2)/30);
roosalyn 0:50d492ea0fd0 100 return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI); // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
roosalyn 0:50d492ea0fd0 101 }
roosalyn 0:50d492ea0fd0 102
roosalyn 0:50d492ea0fd0 103 float bereken_hoek2(float x, float y)
roosalyn 0:50d492ea0fd0 104 {
roosalyn 0:50d492ea0fd0 105 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 0:50d492ea0fd0 106 float alpha = acos((langste_zijde/2)/30);
roosalyn 0:50d492ea0fd0 107 float beta = 0.5*M_PI - alpha;
roosalyn 0:50d492ea0fd0 108 return (2*beta)*4200/(2*M_PI); // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
roosalyn 0:50d492ea0fd0 109 }
roosalyn 0:50d492ea0fd0 110
roosalyn 0:50d492ea0fd0 111 //-------------------------- MOTORCONTROLLERS --------------------------//
roosalyn 0:50d492ea0fd0 112
roosalyn 0:50d492ea0fd0 113 void constanteBepalen()
roosalyn 0:50d492ea0fd0 114 {
roosalyn 0:50d492ea0fd0 115 constante1 = potmeter1.read();
roosalyn 0:50d492ea0fd0 116 constante2 = potmeter2.read();
roosalyn 0:50d492ea0fd0 117 m1_Kd = constante2*0.001; //loopt nu van 0 tot 1
roosalyn 0:50d492ea0fd0 118 m1_Ki = constante1*0.001;
roosalyn 0:50d492ea0fd0 119 }
roosalyn 0:50d492ea0fd0 120
roosalyn 0:50d492ea0fd0 121 double biquad(double u, double &v1, double &v2, const double a1, const double a2,
roosalyn 0:50d492ea0fd0 122 const double b0, const double b1, const double b2){
roosalyn 0:50d492ea0fd0 123 double v = u-a1*v1-a2*v2; //formule uit slides van biquad filter
roosalyn 0:50d492ea0fd0 124 double y = b0*v+b1*v1+b2*v2; //zie ook slides
roosalyn 0:50d492ea0fd0 125 v2 = v1; v1 =v;
roosalyn 0:50d492ea0fd0 126 return y;
roosalyn 0:50d492ea0fd0 127 }
roosalyn 0:50d492ea0fd0 128
roosalyn 0:50d492ea0fd0 129 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,
roosalyn 0:50d492ea0fd0 130 double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
roosalyn 0:50d492ea0fd0 131 const double f_a2, const double f_b0, const double f_b1, const double f_b2){ //In de slides staat f_b3, maar volgens mij moet het f_b2 zijn
roosalyn 0:50d492ea0fd0 132 //Bereken eerst afgeleide van de error
roosalyn 0:50d492ea0fd0 133 double e_der = (e-e_prev)/Ts;
roosalyn 0:50d492ea0fd0 134 double e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); //de y die de biquad returnt is de gefilterde e_der
roosalyn 0:50d492ea0fd0 135 e_prev = e;
roosalyn 0:50d492ea0fd0 136 //Bereken nu de geintegreerde versie van de error
roosalyn 0:50d492ea0fd0 137 e_int = e_int + Ts*e;
roosalyn 0:50d492ea0fd0 138 //PID
roosalyn 0:50d492ea0fd0 139
roosalyn 0:50d492ea0fd0 140 return Kp*e+Ki*e_int+Kd*e_der2; //formule uit slides
roosalyn 0:50d492ea0fd0 141 }
roosalyn 0:50d492ea0fd0 142
roosalyn 0:50d492ea0fd0 143 void motor1_PID_Controller()
roosalyn 0:50d492ea0fd0 144 {
roosalyn 0:50d492ea0fd0 145 //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
roosalyn 0:50d492ea0fd0 146 float hoek1 = bereken_hoek1(countsX,countsY);
roosalyn 0:50d492ea0fd0 147 reference1=hoek1;
roosalyn 0:50d492ea0fd0 148 double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
roosalyn 0:50d492ea0fd0 149
roosalyn 0:50d492ea0fd0 150 scope.set(0,reference1);
roosalyn 0:50d492ea0fd0 151 scope.set(1,position);
roosalyn 0:50d492ea0fd0 152 scope.send();
roosalyn 0:50d492ea0fd0 153
roosalyn 0:50d492ea0fd0 154 difference1 = reference1 - position;
roosalyn 0:50d492ea0fd0 155 constanteBepalen();
roosalyn 0:50d492ea0fd0 156 double u = PID( reference1 - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
roosalyn 0:50d492ea0fd0 157 if (u < 0) {
roosalyn 0:50d492ea0fd0 158 motor1direction = 1; //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
roosalyn 0:50d492ea0fd0 159 } else if (u>=0) {
roosalyn 0:50d492ea0fd0 160 motor1direction = 0;
roosalyn 0:50d492ea0fd0 161 }
roosalyn 0:50d492ea0fd0 162 motor1speed = fabs(u);
roosalyn 0:50d492ea0fd0 163 pc.printf("u = %.2f \r\n", u);
roosalyn 0:50d492ea0fd0 164
roosalyn 0:50d492ea0fd0 165 }
roosalyn 0:50d492ea0fd0 166
roosalyn 0:50d492ea0fd0 167 //-------------------------- POSITIEBEPALING --------------------------//
roosalyn 0:50d492ea0fd0 168
roosalyn 0:50d492ea0fd0 169 void checkButtonStateX()
roosalyn 0:50d492ea0fd0 170 {
roosalyn 0:50d492ea0fd0 171 if (buttonReleasedX(telknopX)) {
roosalyn 0:50d492ea0fd0 172 countStateX = -countStateX;
roosalyn 0:50d492ea0fd0 173 }
roosalyn 0:50d492ea0fd0 174 }
roosalyn 0:50d492ea0fd0 175
roosalyn 0:50d492ea0fd0 176 void checkButtonStateY()
roosalyn 0:50d492ea0fd0 177 {
roosalyn 0:50d492ea0fd0 178 if (buttonReleasedY(telknopY)) {
roosalyn 0:50d492ea0fd0 179 countStateY = -countStateY;
roosalyn 0:50d492ea0fd0 180 }
roosalyn 0:50d492ea0fd0 181 }
roosalyn 0:50d492ea0fd0 182
roosalyn 0:50d492ea0fd0 183 void countStepX()
roosalyn 0:50d492ea0fd0 184 {
roosalyn 0:50d492ea0fd0 185 if (buttonHold(telknopX)) {
roosalyn 0:50d492ea0fd0 186 countsX+= countStateX; //hier moet de richting dus veranderen
roosalyn 0:50d492ea0fd0 187 if(countsX >= 10) {
roosalyn 0:50d492ea0fd0 188 countsX = 10;
roosalyn 0:50d492ea0fd0 189 } else if(countsX<=0) {
roosalyn 0:50d492ea0fd0 190 countsX = 0;
roosalyn 0:50d492ea0fd0 191 }
roosalyn 0:50d492ea0fd0 192 }
roosalyn 0:50d492ea0fd0 193 }
roosalyn 0:50d492ea0fd0 194
roosalyn 0:50d492ea0fd0 195 void countStepY()
roosalyn 0:50d492ea0fd0 196 {
roosalyn 0:50d492ea0fd0 197 if (buttonHold(telknopY)) { //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is
roosalyn 0:50d492ea0fd0 198 countsY+= countStateY;
roosalyn 0:50d492ea0fd0 199 if(countsY >= 10) {
roosalyn 0:50d492ea0fd0 200 countsY = 10;
roosalyn 0:50d492ea0fd0 201 } else if(countsY<=0) {
roosalyn 0:50d492ea0fd0 202 countsY = 0;
roosalyn 0:50d492ea0fd0 203 }
roosalyn 0:50d492ea0fd0 204 }
roosalyn 0:50d492ea0fd0 205 }
roosalyn 0:50d492ea0fd0 206
roosalyn 0:50d492ea0fd0 207 void motor1_PID_Controller_activate(){
roosalyn 0:50d492ea0fd0 208 motor1_PID_Controller_go = true;
roosalyn 0:50d492ea0fd0 209 }
roosalyn 0:50d492ea0fd0 210
roosalyn 0:50d492ea0fd0 211 void countStepX_activate(){
roosalyn 0:50d492ea0fd0 212 countStepX_go = true;
roosalyn 0:50d492ea0fd0 213 }
roosalyn 0:50d492ea0fd0 214
roosalyn 0:50d492ea0fd0 215 void checkButtonStateX_activate(){
roosalyn 0:50d492ea0fd0 216 checkButtonStateX_go = true;
roosalyn 0:50d492ea0fd0 217 }
roosalyn 0:50d492ea0fd0 218
roosalyn 0:50d492ea0fd0 219 void countStepY_activate(){
roosalyn 0:50d492ea0fd0 220 countStepY_go = true;
roosalyn 0:50d492ea0fd0 221 }
roosalyn 0:50d492ea0fd0 222
roosalyn 0:50d492ea0fd0 223 void checkButtonStateY_activate(){
roosalyn 0:50d492ea0fd0 224 checkButtonStateY_go = true;
roosalyn 0:50d492ea0fd0 225 }
roosalyn 0:50d492ea0fd0 226
roosalyn 0:50d492ea0fd0 227 int main()
roosalyn 0:50d492ea0fd0 228 {
roosalyn 0:50d492ea0fd0 229 pc.baud(9600);
roosalyn 0:50d492ea0fd0 230 encoder1.setPosition(0);
roosalyn 0:50d492ea0fd0 231 encoder2.setPosition(0);
roosalyn 0:50d492ea0fd0 232 /*myControllerTicker1.attach( &motor1_PID_Controller, 0.005f ); //Ticker roept elke 0.01 s de motor2 controller aan. = 100Hz
roosalyn 0:50d492ea0fd0 233 myControllerTicker2.attach( &motor2_PI_Controller, 0.01f);
roosalyn 0:50d492ea0fd0 234 countStepTickerX.attach( &countStepX,1.0f );
roosalyn 0:50d492ea0fd0 235 countStepTickerY.attach( &countStepY,1.0f);
roosalyn 0:50d492ea0fd0 236 buttonStateCheckX.attach( &checkButtonStateX,0.001f);
roosalyn 0:50d492ea0fd0 237 buttonStateCheckY.attach( &checkButtonStateY,0.001f);
roosalyn 0:50d492ea0fd0 238 */
roosalyn 0:50d492ea0fd0 239 myControllerTicker1.attach( &motor1_PID_Controller_activate, 0.01f); // Function@ hz
roosalyn 0:50d492ea0fd0 240 countStepTickerX.attach( &countStepX_activate, 1.0f); // Function@ hz
roosalyn 0:50d492ea0fd0 241 countStepTickerY.attach( &countStepY_activate, 1.04f); // Function@ hz
roosalyn 0:50d492ea0fd0 242 buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f); // Function@ hz
roosalyn 0:50d492ea0fd0 243 buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f); // Function@ hz
roosalyn 0:50d492ea0fd0 244
roosalyn 0:50d492ea0fd0 245
roosalyn 0:50d492ea0fd0 246 while (true) {
roosalyn 0:50d492ea0fd0 247
roosalyn 0:50d492ea0fd0 248 pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Kd,m1_Ki,countsX,countsY);
roosalyn 0:50d492ea0fd0 249 //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
roosalyn 0:50d492ea0fd0 250
roosalyn 0:50d492ea0fd0 251 if(motor1_PID_Controller_go) {
roosalyn 0:50d492ea0fd0 252 motor1_PID_Controller_go = false;
roosalyn 0:50d492ea0fd0 253 motor1_PID_Controller();
roosalyn 0:50d492ea0fd0 254 }
roosalyn 0:50d492ea0fd0 255 if(countStepX_go) {
roosalyn 0:50d492ea0fd0 256 countStepX_go = false;
roosalyn 0:50d492ea0fd0 257 countStepX();
roosalyn 0:50d492ea0fd0 258 }
roosalyn 0:50d492ea0fd0 259 if(countStepY_go) {
roosalyn 0:50d492ea0fd0 260 countStepY_go = false;
roosalyn 0:50d492ea0fd0 261 countStepY();
roosalyn 0:50d492ea0fd0 262 }
roosalyn 0:50d492ea0fd0 263 if(checkButtonStateX_go) {
roosalyn 0:50d492ea0fd0 264 checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 265 checkButtonStateX();
roosalyn 0:50d492ea0fd0 266 }
roosalyn 0:50d492ea0fd0 267 if(checkButtonStateY_go) {
roosalyn 0:50d492ea0fd0 268 checkButtonStateY_go = false;
roosalyn 0:50d492ea0fd0 269 checkButtonStateY();
roosalyn 0:50d492ea0fd0 270 }
roosalyn 0:50d492ea0fd0 271 }
roosalyn 0:50d492ea0fd0 272 }