Laura Corrales / Mbed 2 deprecated PID_Encoder

Controlador PID con lectura mediante encoder

Dependencies:   DebouncedIn QEI TextLCD_encoder mbed

main.cpp@3:9347b362d33c, 2013-12-15 (annotated)

Committer:
lcorralesc1
Date:
Sun Dec 15 15:57:03 2013 +0000
Revision:
3:9347b362d33c
Parent:
2:fa1d90037a6c 
This program makes a PID control and the values for control action are entered by quadrature encoder.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lcorralesc1 0:8d2bbee60422 1 #include "mbed.h"
lcorralesc1 0:8d2bbee60422 2 #include "DebouncedIn.h"
lcorralesc1 0:8d2bbee60422 3 #include "TextLCD.h"
lcorralesc1 0:8d2bbee60422 4 #include "QEI.h"
lcorralesc1 0:8d2bbee60422 5
lcorralesc1 0:8d2bbee60422 6
lcorralesc1 0:8d2bbee60422 7 AnalogIn Vin(PTC2);
lcorralesc1 0:8d2bbee60422 8 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
lcorralesc1 2:fa1d90037a6c 9 QEI wheel (PTD5, PTD0, NC, 100);
lcorralesc1 0:8d2bbee60422 10
lcorralesc1 3:9347b362d33c 11 AnalogIn y(PTB1);
lcorralesc1 0:8d2bbee60422 12 AnalogOut u(PTE30);
lcorralesc1 0:8d2bbee60422 13
lcorralesc1 0:8d2bbee60422 14 DigitalOut led1(LED1);
lcorralesc1 0:8d2bbee60422 15 DigitalOut led2(LED2);
lcorralesc1 0:8d2bbee60422 16 DigitalOut led3(LED3);
lcorralesc1 0:8d2bbee60422 17 DebouncedIn button1(PTC12);
lcorralesc1 0:8d2bbee60422 18 DebouncedIn button2(PTC13);
lcorralesc1 0:8d2bbee60422 19 DebouncedIn button3(PTC16);
lcorralesc1 0:8d2bbee60422 20 DebouncedIn button4(PTC17);
lcorralesc1 0:8d2bbee60422 21
lcorralesc1 0:8d2bbee60422 22 //codigos movimiento del curzor
lcorralesc1 0:8d2bbee60422 23 //18 para izquierda
lcorralesc1 0:8d2bbee60422 24 //1A para derecha
lcorralesc1 0:8d2bbee60422 25
lcorralesc1 0:8d2bbee60422 26 //int C1=0x0E; // solo muestra el curzor
lcorralesc1 0:8d2bbee60422 27 int C2=0x18; // desplaza izquierda
lcorralesc1 0:8d2bbee60422 28 int C3=0x1A; // desplaza derecha
lcorralesc1 0:8d2bbee60422 29 int C4=0x0C; // quito cursor bajo
lcorralesc1 0:8d2bbee60422 30 int C1=0x0F;
lcorralesc1 0:8d2bbee60422 31
lcorralesc1 0:8d2bbee60422 32 int i; // indice de la variable
lcorralesc1 0:8d2bbee60422 33 int j; //variable controla cambio 4 posiciones
lcorralesc1 3:9347b362d33c 34 int kp, ki, kd, sp, yr, ap, ai, ad, err, med, err_v, cycle, pid;
lcorralesc1 3:9347b362d33c 35 int b=0;
lcorralesc1 0:8d2bbee60422 36 float pidn;
lcorralesc1 3:9347b362d33c 37 Timer t;
lcorralesc1 0:8d2bbee60422 38
lcorralesc1 0:8d2bbee60422 39 int main() {
lcorralesc1 0:8d2bbee60422 40 lcd.cls();
lcorralesc1 0:8d2bbee60422 41 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
lcorralesc1 0:8d2bbee60422 42 lcd.locate(0,0);
lcorralesc1 0:8d2bbee60422 43 lcd.printf("Sp%d",sp);
lcorralesc1 0:8d2bbee60422 44 lcd.locate(8,0);
lcorralesc1 0:8d2bbee60422 45 lcd.printf("Kp%d",kp);
lcorralesc1 0:8d2bbee60422 46 lcd.locate(0,1);
lcorralesc1 0:8d2bbee60422 47 lcd.printf("Ki%d",ki);
lcorralesc1 0:8d2bbee60422 48 lcd.locate(8,1);
lcorralesc1 0:8d2bbee60422 49 lcd.printf("Kd%d",kd);
lcorralesc1 0:8d2bbee60422 50
lcorralesc1 0:8d2bbee60422 51
lcorralesc1 0:8d2bbee60422 52 while(1) {
lcorralesc1 0:8d2bbee60422 53
lcorralesc1 0:8d2bbee60422 54 led3 =1;
lcorralesc1 2:fa1d90037a6c 55 if (button3.falling()) { //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
lcorralesc1 0:8d2bbee60422 56 led3 =!led3;
lcorralesc1 0:8d2bbee60422 57 ++j;
lcorralesc1 0:8d2bbee60422 58 }
lcorralesc1 2:fa1d90037a6c 59
lcorralesc1 0:8d2bbee60422 60 if (j==0){
lcorralesc1 0:8d2bbee60422 61 sp=sp+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 62 wheel.reset();
lcorralesc1 0:8d2bbee60422 63 if (sp>999){
lcorralesc1 0:8d2bbee60422 64 sp=999;
lcorralesc1 0:8d2bbee60422 65 }
lcorralesc1 0:8d2bbee60422 66 if (sp<0){
lcorralesc1 0:8d2bbee60422 67 sp=0;
lcorralesc1 0:8d2bbee60422 68 }
lcorralesc1 0:8d2bbee60422 69 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 70 lcd.printf(" ",sp);
lcorralesc1 0:8d2bbee60422 71 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 72 lcd.printf("%d",sp);
lcorralesc1 0:8d2bbee60422 73 wait(0.2);
lcorralesc1 0:8d2bbee60422 74
lcorralesc1 0:8d2bbee60422 75 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 76 j=1;
lcorralesc1 0:8d2bbee60422 77 led3=0;
lcorralesc1 0:8d2bbee60422 78 wait(0.3);
lcorralesc1 0:8d2bbee60422 79 wheel.reset();
lcorralesc1 0:8d2bbee60422 80 }
lcorralesc1 0:8d2bbee60422 81
lcorralesc1 0:8d2bbee60422 82 }
lcorralesc1 0:8d2bbee60422 83
lcorralesc1 0:8d2bbee60422 84 if (j==1) {
lcorralesc1 0:8d2bbee60422 85 kp=kp+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 86 wheel.reset();
lcorralesc1 0:8d2bbee60422 87 if (kp>999){
lcorralesc1 0:8d2bbee60422 88 kp=999;
lcorralesc1 0:8d2bbee60422 89 }
lcorralesc1 0:8d2bbee60422 90 if (kp<0){
lcorralesc1 0:8d2bbee60422 91 kp=0;
lcorralesc1 0:8d2bbee60422 92 }
lcorralesc1 0:8d2bbee60422 93 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 94 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 95 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 96 lcd.printf("%d",kp);
lcorralesc1 0:8d2bbee60422 97 wait(0.2);
lcorralesc1 0:8d2bbee60422 98
lcorralesc1 0:8d2bbee60422 99 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 100 j=2;
lcorralesc1 0:8d2bbee60422 101 led3=0;
lcorralesc1 0:8d2bbee60422 102 wait(0.3);
lcorralesc1 0:8d2bbee60422 103 wheel.reset();
lcorralesc1 0:8d2bbee60422 104 }
lcorralesc1 0:8d2bbee60422 105
lcorralesc1 0:8d2bbee60422 106 }
lcorralesc1 0:8d2bbee60422 107
lcorralesc1 0:8d2bbee60422 108 if (j==2) {
lcorralesc1 0:8d2bbee60422 109 ki=ki+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 110 wheel.reset();
lcorralesc1 0:8d2bbee60422 111 if (ki>999){
lcorralesc1 0:8d2bbee60422 112 ki=999;
lcorralesc1 0:8d2bbee60422 113 }
lcorralesc1 0:8d2bbee60422 114 if (ki<0){
lcorralesc1 0:8d2bbee60422 115 ki=0;
lcorralesc1 0:8d2bbee60422 116 }
lcorralesc1 0:8d2bbee60422 117 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 118 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 119 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 120 lcd.printf("%d",ki);
lcorralesc1 0:8d2bbee60422 121 wait(0.2);
lcorralesc1 0:8d2bbee60422 122
lcorralesc1 0:8d2bbee60422 123 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 124 j=3;
lcorralesc1 0:8d2bbee60422 125 led3=0;
lcorralesc1 0:8d2bbee60422 126 wait(0.3);
lcorralesc1 0:8d2bbee60422 127 wheel.reset();
lcorralesc1 0:8d2bbee60422 128 }
lcorralesc1 0:8d2bbee60422 129
lcorralesc1 0:8d2bbee60422 130 }
lcorralesc1 0:8d2bbee60422 131
lcorralesc1 0:8d2bbee60422 132 if (j==3) {
lcorralesc1 0:8d2bbee60422 133 kd=kd+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 134 wheel.reset();
lcorralesc1 0:8d2bbee60422 135 if (kd>999){
lcorralesc1 0:8d2bbee60422 136 kd=999;
lcorralesc1 0:8d2bbee60422 137 }
lcorralesc1 0:8d2bbee60422 138 if (kd<0){
lcorralesc1 0:8d2bbee60422 139 kd=0;
lcorralesc1 0:8d2bbee60422 140 }
lcorralesc1 0:8d2bbee60422 141 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 142 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 143 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 144 lcd.printf("%d",kd);
lcorralesc1 0:8d2bbee60422 145 wait(0.2);
lcorralesc1 0:8d2bbee60422 146
lcorralesc1 0:8d2bbee60422 147 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 148 j=0;
lcorralesc1 0:8d2bbee60422 149 led3=0;
lcorralesc1 0:8d2bbee60422 150 wait(0.3);
lcorralesc1 0:8d2bbee60422 151 wheel.reset();
lcorralesc1 0:8d2bbee60422 152 }
lcorralesc1 0:8d2bbee60422 153
lcorralesc1 0:8d2bbee60422 154 }
lcorralesc1 0:8d2bbee60422 155
lcorralesc1 0:8d2bbee60422 156 if (j==4) {
lcorralesc1 0:8d2bbee60422 157 j=0;
lcorralesc1 0:8d2bbee60422 158 }
lcorralesc1 0:8d2bbee60422 159
lcorralesc1 0:8d2bbee60422 160 if (!button4){
lcorralesc1 0:8d2bbee60422 161 break; //sale del bucle si pisan suiche4
lcorralesc1 0:8d2bbee60422 162 }
lcorralesc1 0:8d2bbee60422 163 } //cierro while(1)
lcorralesc1 0:8d2bbee60422 164 //%---------------------------------------------------------------------
lcorralesc1 0:8d2bbee60422 165
lcorralesc1 0:8d2bbee60422 166
lcorralesc1 0:8d2bbee60422 167 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
lcorralesc1 0:8d2bbee60422 168 lcd.cls(); //borra la pantalla
lcorralesc1 0:8d2bbee60422 169 lcd.printf("GUARDAMOS \nVALORES |m|");
lcorralesc1 0:8d2bbee60422 170 wait(2);
lcorralesc1 0:8d2bbee60422 171
lcorralesc1 0:8d2bbee60422 172 // se imprimen los parches del control *****************************************
lcorralesc1 0:8d2bbee60422 173 lcd.cls();
lcorralesc1 0:8d2bbee60422 174 lcd.printf("Er%d",err);
lcorralesc1 0:8d2bbee60422 175 lcd.locate(8,0);
lcorralesc1 0:8d2bbee60422 176 lcd.printf("Me%d",med);
lcorralesc1 0:8d2bbee60422 177 lcd.locate(0,1);
lcorralesc1 0:8d2bbee60422 178 lcd.printf("Sp%d",sp);
lcorralesc1 0:8d2bbee60422 179 lcd.locate(8,1);
lcorralesc1 0:8d2bbee60422 180 lcd.printf("Co%d",pid);
lcorralesc1 3:9347b362d33c 181 wait(1);
lcorralesc1 0:8d2bbee60422 182
lcorralesc1 0:8d2bbee60422 183
lcorralesc1 0:8d2bbee60422 184 // CICLO PRINCIPAL CONTROLADOR PID
lcorralesc1 3:9347b362d33c 185 b=0;
lcorralesc1 0:8d2bbee60422 186 while(1) {
lcorralesc1 0:8d2bbee60422 187 med=999*y.read(); //leer puerto analogo y asignar a med
lcorralesc1 0:8d2bbee60422 188 err = (sp-med);
lcorralesc1 0:8d2bbee60422 189 ap = kp*err;
lcorralesc1 0:8d2bbee60422 190
lcorralesc1 0:8d2bbee60422 191 // se verifica que la accion integral no sea muy grande
lcorralesc1 3:9347b362d33c 192 if(ai<100)
lcorralesc1 3:9347b362d33c 193 {
lcorralesc1 0:8d2bbee60422 194 ai =(ki*err)+ai; //calculo de la integral del error
lcorralesc1 3:9347b362d33c 195 }
lcorralesc1 2:fa1d90037a6c 196 //else{
lcorralesc1 0:8d2bbee60422 197 //Dejo de sumar la accion integral
lcorralesc1 2:fa1d90037a6c 198 // }
lcorralesc1 0:8d2bbee60422 199
lcorralesc1 0:8d2bbee60422 200 ad = kd*(err-err_v); //calculo de la accion derivativa
lcorralesc1 0:8d2bbee60422 201 pid = (ap+ai+ad);
lcorralesc1 0:8d2bbee60422 202
lcorralesc1 3:9347b362d33c 203
lcorralesc1 0:8d2bbee60422 204
lcorralesc1 0:8d2bbee60422 205 // se verifica que pid sea positivo **************************************
lcorralesc1 3:9347b362d33c 206 if(pid<=0)
lcorralesc1 3:9347b362d33c 207 {
lcorralesc1 0:8d2bbee60422 208 pid=0;
lcorralesc1 3:9347b362d33c 209 }
lcorralesc1 0:8d2bbee60422 210 // se verifica que pid sea menor o igual la valor maximo *****************
lcorralesc1 3:9347b362d33c 211 if (pid > 5000)
lcorralesc1 3:9347b362d33c 212 {
lcorralesc1 3:9347b362d33c 213 pid=5000;
lcorralesc1 3:9347b362d33c 214 }
lcorralesc1 0:8d2bbee60422 215
lcorralesc1 0:8d2bbee60422 216 // se actualizan las variables *******************************************
lcorralesc1 3:9347b362d33c 217 err_v = err;
lcorralesc1 0:8d2bbee60422 218
lcorralesc1 0:8d2bbee60422 219 //se muestran las variables******************************************
lcorralesc1 0:8d2bbee60422 220
lcorralesc1 3:9347b362d33c 221 if (b==0)
lcorralesc1 3:9347b362d33c 222 {
lcorralesc1 3:9347b362d33c 223 t.start();
lcorralesc1 3:9347b362d33c 224 b=1;
lcorralesc1 3:9347b362d33c 225 }
lcorralesc1 3:9347b362d33c 226 if(t>=0.3)
lcorralesc1 3:9347b362d33c 227 {
lcorralesc1 3:9347b362d33c 228 wait(0.2);
lcorralesc1 2:fa1d90037a6c 229 lcd.locate(2,0);
lcorralesc1 2:fa1d90037a6c 230 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 231 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 232 lcd.printf("%d",err);
lcorralesc1 0:8d2bbee60422 233 lcd.locate(10,0);
lcorralesc1 2:fa1d90037a6c 234 lcd.printf(" ");
lcorralesc1 2:fa1d90037a6c 235 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 236 lcd.printf("%d",med);
lcorralesc1 0:8d2bbee60422 237 lcd.locate(2,1);
lcorralesc1 2:fa1d90037a6c 238 lcd.printf(" ");
lcorralesc1 2:fa1d90037a6c 239 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 240 lcd.printf("%d",sp);
lcorralesc1 0:8d2bbee60422 241 lcd.locate(10,1);
lcorralesc1 2:fa1d90037a6c 242 lcd.printf(" ");
lcorralesc1 2:fa1d90037a6c 243 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 244 lcd.printf("%d",pid);
lcorralesc1 3:9347b362d33c 245 }
lcorralesc1 0:8d2bbee60422 246
lcorralesc1 0:8d2bbee60422 247 //Normalizacion de la salida
lcorralesc1 0:8d2bbee60422 248 pidn=pid/999;
lcorralesc1 0:8d2bbee60422 249 // se envia el valor pid a puerto analogico de salida (D/A) **************
lcorralesc1 0:8d2bbee60422 250 u.write(pidn);
lcorralesc1 3:9347b362d33c 251
lcorralesc1 0:8d2bbee60422 252 // se repite el ciclo
lcorralesc1 3:9347b362d33c 253 wait(1.5);
lcorralesc1 0:8d2bbee60422 254 }
lcorralesc1 2:fa1d90037a6c 255
lcorralesc1 2:fa1d90037a6c 256
lcorralesc1 2:fa1d90037a6c 257
lcorralesc1 3:9347b362d33c 258 }
lcorralesc1 3:9347b362d33c 259