Laura Corrales / Mbed 2 deprecated PID_Encoder

Controlador PID con lectura mediante encoder

Dependencies:   DebouncedIn QEI TextLCD_encoder mbed

main.cpp@0:8d2bbee60422, 2013-11-09 (annotated)

Committer:
lcorralesc1
Date:
Sat Nov 09 18:09:11 2013 +0000
Revision:
0:8d2bbee60422
Child:
1:ffbcc55fa659
Controlador PID con lectura mediante 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 0:8d2bbee60422 9 QEI wheel (PTD5, PTD0, NC, 624);
lcorralesc1 0:8d2bbee60422 10
lcorralesc1 0:8d2bbee60422 11 AnalogIn y(PTB0);
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 0:8d2bbee60422 34 int kp, ki, kd, sp, err, med, yr, ap, ai, ad, err_v, cycle, pid;
lcorralesc1 0:8d2bbee60422 35 float pidn;
lcorralesc1 0:8d2bbee60422 36
lcorralesc1 0:8d2bbee60422 37 int main() {
lcorralesc1 0:8d2bbee60422 38 lcd.cls();
lcorralesc1 0:8d2bbee60422 39 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
lcorralesc1 0:8d2bbee60422 40 lcd.locate(0,0);
lcorralesc1 0:8d2bbee60422 41 lcd.printf("Sp%d",sp);
lcorralesc1 0:8d2bbee60422 42 lcd.locate(8,0);
lcorralesc1 0:8d2bbee60422 43 lcd.printf("Kp%d",kp);
lcorralesc1 0:8d2bbee60422 44 lcd.locate(0,1);
lcorralesc1 0:8d2bbee60422 45 lcd.printf("Ki%d",ki);
lcorralesc1 0:8d2bbee60422 46 lcd.locate(8,1);
lcorralesc1 0:8d2bbee60422 47 lcd.printf("Kd%d",kd);
lcorralesc1 0:8d2bbee60422 48
lcorralesc1 0:8d2bbee60422 49
lcorralesc1 0:8d2bbee60422 50 while(1) {
lcorralesc1 0:8d2bbee60422 51
lcorralesc1 0:8d2bbee60422 52 led3 =1;
lcorralesc1 0:8d2bbee60422 53 if (button3.falling()) {
lcorralesc1 0:8d2bbee60422 54 led3 =!led3;
lcorralesc1 0:8d2bbee60422 55 ++j;
lcorralesc1 0:8d2bbee60422 56 }
lcorralesc1 0:8d2bbee60422 57 //INCREMENTA POSICION DEL MENU CON BOTON 3
lcorralesc1 0:8d2bbee60422 58 if (j==0){
lcorralesc1 0:8d2bbee60422 59 sp=sp+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 60 wheel.reset();
lcorralesc1 0:8d2bbee60422 61 if (sp>999){
lcorralesc1 0:8d2bbee60422 62 sp=999;
lcorralesc1 0:8d2bbee60422 63 }
lcorralesc1 0:8d2bbee60422 64 if (sp<0){
lcorralesc1 0:8d2bbee60422 65 sp=0;
lcorralesc1 0:8d2bbee60422 66 }
lcorralesc1 0:8d2bbee60422 67 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 68 lcd.printf(" ",sp);
lcorralesc1 0:8d2bbee60422 69 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 70 lcd.printf("%d",sp);
lcorralesc1 0:8d2bbee60422 71 wait(0.2);
lcorralesc1 0:8d2bbee60422 72
lcorralesc1 0:8d2bbee60422 73 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 74 j=1;
lcorralesc1 0:8d2bbee60422 75 led3=0;
lcorralesc1 0:8d2bbee60422 76 wait(0.3);
lcorralesc1 0:8d2bbee60422 77 wheel.reset();
lcorralesc1 0:8d2bbee60422 78 }
lcorralesc1 0:8d2bbee60422 79
lcorralesc1 0:8d2bbee60422 80 }
lcorralesc1 0:8d2bbee60422 81
lcorralesc1 0:8d2bbee60422 82 if (j==1) {
lcorralesc1 0:8d2bbee60422 83 kp=kp+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 84 wheel.reset();
lcorralesc1 0:8d2bbee60422 85 if (kp>999){
lcorralesc1 0:8d2bbee60422 86 kp=999;
lcorralesc1 0:8d2bbee60422 87 }
lcorralesc1 0:8d2bbee60422 88 if (kp<0){
lcorralesc1 0:8d2bbee60422 89 kp=0;
lcorralesc1 0:8d2bbee60422 90 }
lcorralesc1 0:8d2bbee60422 91 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 92 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 93 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 94 lcd.printf("%d",kp);
lcorralesc1 0:8d2bbee60422 95 wait(0.2);
lcorralesc1 0:8d2bbee60422 96
lcorralesc1 0:8d2bbee60422 97 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 98 j=2;
lcorralesc1 0:8d2bbee60422 99 led3=0;
lcorralesc1 0:8d2bbee60422 100 wait(0.3);
lcorralesc1 0:8d2bbee60422 101 wheel.reset();
lcorralesc1 0:8d2bbee60422 102 }
lcorralesc1 0:8d2bbee60422 103
lcorralesc1 0:8d2bbee60422 104 }
lcorralesc1 0:8d2bbee60422 105
lcorralesc1 0:8d2bbee60422 106 if (j==2) {
lcorralesc1 0:8d2bbee60422 107 ki=ki+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 108 wheel.reset();
lcorralesc1 0:8d2bbee60422 109 if (ki>999){
lcorralesc1 0:8d2bbee60422 110 ki=999;
lcorralesc1 0:8d2bbee60422 111 }
lcorralesc1 0:8d2bbee60422 112 if (ki<0){
lcorralesc1 0:8d2bbee60422 113 ki=0;
lcorralesc1 0:8d2bbee60422 114 }
lcorralesc1 0:8d2bbee60422 115 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 116 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 117 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 118 lcd.printf("%d",ki);
lcorralesc1 0:8d2bbee60422 119 wait(0.2);
lcorralesc1 0:8d2bbee60422 120
lcorralesc1 0:8d2bbee60422 121 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 122 j=3;
lcorralesc1 0:8d2bbee60422 123 led3=0;
lcorralesc1 0:8d2bbee60422 124 wait(0.3);
lcorralesc1 0:8d2bbee60422 125 wheel.reset();
lcorralesc1 0:8d2bbee60422 126 }
lcorralesc1 0:8d2bbee60422 127
lcorralesc1 0:8d2bbee60422 128 }
lcorralesc1 0:8d2bbee60422 129
lcorralesc1 0:8d2bbee60422 130 if (j==3) {
lcorralesc1 0:8d2bbee60422 131 kd=kd+wheel.getPulses();
lcorralesc1 0:8d2bbee60422 132 wheel.reset();
lcorralesc1 0:8d2bbee60422 133 if (kd>999){
lcorralesc1 0:8d2bbee60422 134 kd=999;
lcorralesc1 0:8d2bbee60422 135 }
lcorralesc1 0:8d2bbee60422 136 if (kd<0){
lcorralesc1 0:8d2bbee60422 137 kd=0;
lcorralesc1 0:8d2bbee60422 138 }
lcorralesc1 0:8d2bbee60422 139 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 140 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 141 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 142 lcd.printf("%d",kd);
lcorralesc1 0:8d2bbee60422 143 wait(0.2);
lcorralesc1 0:8d2bbee60422 144
lcorralesc1 0:8d2bbee60422 145 if(button3.falling()){
lcorralesc1 0:8d2bbee60422 146 j=0;
lcorralesc1 0:8d2bbee60422 147 led3=0;
lcorralesc1 0:8d2bbee60422 148 wait(0.3);
lcorralesc1 0:8d2bbee60422 149 wheel.reset();
lcorralesc1 0:8d2bbee60422 150 }
lcorralesc1 0:8d2bbee60422 151
lcorralesc1 0:8d2bbee60422 152 }
lcorralesc1 0:8d2bbee60422 153
lcorralesc1 0:8d2bbee60422 154 if (j==4) {
lcorralesc1 0:8d2bbee60422 155 j=0;
lcorralesc1 0:8d2bbee60422 156 }
lcorralesc1 0:8d2bbee60422 157
lcorralesc1 0:8d2bbee60422 158 if (!button4){
lcorralesc1 0:8d2bbee60422 159 break; //sale del bucle si pisan suiche4
lcorralesc1 0:8d2bbee60422 160 }
lcorralesc1 0:8d2bbee60422 161 } //cierro while(1)
lcorralesc1 0:8d2bbee60422 162 //%---------------------------------------------------------------------
lcorralesc1 0:8d2bbee60422 163
lcorralesc1 0:8d2bbee60422 164
lcorralesc1 0:8d2bbee60422 165 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
lcorralesc1 0:8d2bbee60422 166 lcd.cls(); //borra la pantalla
lcorralesc1 0:8d2bbee60422 167 lcd.printf("GUARDAMOS \nVALORES |m|");
lcorralesc1 0:8d2bbee60422 168 wait(2);
lcorralesc1 0:8d2bbee60422 169
lcorralesc1 0:8d2bbee60422 170 // se imprimen los parches del control *****************************************
lcorralesc1 0:8d2bbee60422 171 lcd.cls();
lcorralesc1 0:8d2bbee60422 172 lcd.printf("Er%d",err);
lcorralesc1 0:8d2bbee60422 173 lcd.locate(8,0);
lcorralesc1 0:8d2bbee60422 174 lcd.printf("Me%d",med);
lcorralesc1 0:8d2bbee60422 175 lcd.locate(0,1);
lcorralesc1 0:8d2bbee60422 176 lcd.printf("Sp%d",sp);
lcorralesc1 0:8d2bbee60422 177 lcd.locate(8,1);
lcorralesc1 0:8d2bbee60422 178 lcd.printf("Co%d",pid);
lcorralesc1 0:8d2bbee60422 179 wait(5);
lcorralesc1 0:8d2bbee60422 180
lcorralesc1 0:8d2bbee60422 181
lcorralesc1 0:8d2bbee60422 182 // CICLO PRINCIPAL CONTROLADOR PID
lcorralesc1 0:8d2bbee60422 183
lcorralesc1 0:8d2bbee60422 184 while(1) {
lcorralesc1 0:8d2bbee60422 185 med=999*y.read(); //leer puerto analogo y asignar a med
lcorralesc1 0:8d2bbee60422 186 err = (sp-med);
lcorralesc1 0:8d2bbee60422 187 ap = kp*err;
lcorralesc1 0:8d2bbee60422 188
lcorralesc1 0:8d2bbee60422 189 // se verifica que la accion integral no sea muy grande
lcorralesc1 0:8d2bbee60422 190 if(ai<100){
lcorralesc1 0:8d2bbee60422 191 ai =(ki*err)+ai; //calculo de la integral del error
lcorralesc1 0:8d2bbee60422 192 }
lcorralesc1 0:8d2bbee60422 193 else{
lcorralesc1 0:8d2bbee60422 194 //Dejo de sumar la accion integral
lcorralesc1 0:8d2bbee60422 195 }
lcorralesc1 0:8d2bbee60422 196
lcorralesc1 0:8d2bbee60422 197 ad = kd*(err-err_v); //calculo de la accion derivativa
lcorralesc1 0:8d2bbee60422 198 pid = (ap+ai+ad);
lcorralesc1 0:8d2bbee60422 199
lcorralesc1 0:8d2bbee60422 200 // se actualizan las variables *******************************************
lcorralesc1 0:8d2bbee60422 201 err_v = err;
lcorralesc1 0:8d2bbee60422 202
lcorralesc1 0:8d2bbee60422 203 // se verifica que pid sea positivo **************************************
lcorralesc1 0:8d2bbee60422 204 if(pid<=0){
lcorralesc1 0:8d2bbee60422 205 pid=0;
lcorralesc1 0:8d2bbee60422 206 }
lcorralesc1 0:8d2bbee60422 207 // se verifica que pid sea menor o igual la valor maximo *****************
lcorralesc1 0:8d2bbee60422 208 if (pid > 999){
lcorralesc1 0:8d2bbee60422 209 pid=999;
lcorralesc1 0:8d2bbee60422 210 }
lcorralesc1 0:8d2bbee60422 211
lcorralesc1 0:8d2bbee60422 212 // se actualizan las variables *******************************************
lcorralesc1 0:8d2bbee60422 213 err_v = err;
lcorralesc1 0:8d2bbee60422 214
lcorralesc1 0:8d2bbee60422 215 //se muestran las variables******************************************
lcorralesc1 0:8d2bbee60422 216
lcorralesc1 0:8d2bbee60422 217 wait(0.3);
lcorralesc1 0:8d2bbee60422 218 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 219 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 220 lcd.locate(2,0);
lcorralesc1 0:8d2bbee60422 221 lcd.printf("%d",err);
lcorralesc1 0:8d2bbee60422 222 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 223 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 224 lcd.locate(10,0);
lcorralesc1 0:8d2bbee60422 225 lcd.printf("%d",med);
lcorralesc1 0:8d2bbee60422 226 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 227 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 228 lcd.locate(2,1);
lcorralesc1 0:8d2bbee60422 229 lcd.printf("%d",sp);
lcorralesc1 0:8d2bbee60422 230 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 231 lcd.printf(" ");
lcorralesc1 0:8d2bbee60422 232 lcd.locate(10,1);
lcorralesc1 0:8d2bbee60422 233 lcd.printf("%d",pid);
lcorralesc1 0:8d2bbee60422 234
lcorralesc1 0:8d2bbee60422 235
lcorralesc1 0:8d2bbee60422 236 //Normalizacion de la salida
lcorralesc1 0:8d2bbee60422 237 pidn=pid/999;
lcorralesc1 0:8d2bbee60422 238 // se envia el valor pid a puerto analogico de salida (D/A) **************
lcorralesc1 0:8d2bbee60422 239 u.write(pidn);
lcorralesc1 0:8d2bbee60422 240 // se repite el ciclo
lcorralesc1 0:8d2bbee60422 241 }
lcorralesc1 0:8d2bbee60422 242
lcorralesc1 0:8d2bbee60422 243
lcorralesc1 0:8d2bbee60422 244
lcorralesc1 0:8d2bbee60422 245 }