Alejandro Marin / Mbed 2 deprecated Tarea4Encoder

En este programa se presenta un PID modificando los valores a traves de un encoder

Dependencies:   QEI TextLCD mbed

/media/uploads/amarincan/pid-encoder.jpg

main.cpp@0:fb37cd782170, 2013-11-13 (annotated)

Committer:
amarincan
Date:
Wed Nov 13 02:59:07 2013 +0000
Revision:
0:fb37cd782170
PID con encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amarincan 0:fb37cd782170 1 #include "mbed.h"
amarincan 0:fb37cd782170 2 #include "DebouncedIn.h"
amarincan 0:fb37cd782170 3 #include "TextLCD.h"
amarincan 0:fb37cd782170 4 #include "QEI.h"
amarincan 0:fb37cd782170 5
amarincan 0:fb37cd782170 6 AnalogIn Vin(PTC2);
amarincan 0:fb37cd782170 7 AnalogOut Vout(PTE30);
amarincan 0:fb37cd782170 8
amarincan 0:fb37cd782170 9 QEI wheel (PTA16, PTA17, NC, 24);
amarincan 0:fb37cd782170 10 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
amarincan 0:fb37cd782170 11
amarincan 0:fb37cd782170 12 DigitalOut led1(LED1);
amarincan 0:fb37cd782170 13 DigitalOut led2(LED2);
amarincan 0:fb37cd782170 14 DebouncedIn button1(PTC12);
amarincan 0:fb37cd782170 15 DebouncedIn button2(PTC13);
amarincan 0:fb37cd782170 16 DebouncedIn button3(PTC16);
amarincan 0:fb37cd782170 17 DebouncedIn button4(PTC17);
amarincan 0:fb37cd782170 18
amarincan 0:fb37cd782170 19 /* codigos movimiento del curzor
amarincan 0:fb37cd782170 20 18 para izquierda
amarincan 0:fb37cd782170 21 1A para derecha
amarincan 0:fb37cd782170 22 */
amarincan 0:fb37cd782170 23
amarincan 0:fb37cd782170 24
amarincan 0:fb37cd782170 25 int C1=0x0E; // solo muestra el curzor
amarincan 0:fb37cd782170 26 int C2=0x18; // desplaza izquierda
amarincan 0:fb37cd782170 27 int C3=0x1A; // desplaza derecha
amarincan 0:fb37cd782170 28 int C4=0x0C; // quito cursor bajo
amarincan 0:fb37cd782170 29 int i; // indice de la variable
amarincan 0:fb37cd782170 30 int j;
amarincan 0:fb37cd782170 31 int Kp, Ki, Kd, Sp, yr, cycle, med2 , c,v,m;
amarincan 0:fb37cd782170 32 float med,pid,ap, err, ai, ad,err_v;
amarincan 0:fb37cd782170 33 int main() {
amarincan 0:fb37cd782170 34
amarincan 0:fb37cd782170 35 lcd.writeCommand(0x0E);
amarincan 0:fb37cd782170 36 lcd.printf("Sp=0 Kp=0");
amarincan 0:fb37cd782170 37 lcd.locate(0,1);
amarincan 0:fb37cd782170 38 lcd.printf("Ki=0 Kd=0");
amarincan 0:fb37cd782170 39 lcd.locate(2,0);
amarincan 0:fb37cd782170 40 lcd.printf("=");
amarincan 0:fb37cd782170 41
amarincan 0:fb37cd782170 42 while(1) {
amarincan 0:fb37cd782170 43
amarincan 0:fb37cd782170 44 m=wheel.getPulses();
amarincan 0:fb37cd782170 45 if(m<0){
amarincan 0:fb37cd782170 46 wheel.reset();
amarincan 0:fb37cd782170 47 m=0;
amarincan 0:fb37cd782170 48 }
amarincan 0:fb37cd782170 49 if(m!=v) {
amarincan 0:fb37cd782170 50
amarincan 0:fb37cd782170 51 switch(i) {
amarincan 0:fb37cd782170 52
amarincan 0:fb37cd782170 53 case 0:
amarincan 0:fb37cd782170 54
amarincan 0:fb37cd782170 55 lcd.locate(2,0);
amarincan 0:fb37cd782170 56 lcd.printf("= ");
amarincan 0:fb37cd782170 57 lcd.locate(3,0);
amarincan 0:fb37cd782170 58 lcd.printf("%d",m);
amarincan 0:fb37cd782170 59 Sp=m;
amarincan 0:fb37cd782170 60
amarincan 0:fb37cd782170 61 break;
amarincan 0:fb37cd782170 62 case 1:
amarincan 0:fb37cd782170 63
amarincan 0:fb37cd782170 64 lcd.locate(10,0);
amarincan 0:fb37cd782170 65 lcd.printf("= ");
amarincan 0:fb37cd782170 66 lcd.locate(11,0);
amarincan 0:fb37cd782170 67 lcd.printf("%d",m);
amarincan 0:fb37cd782170 68 Kp=m;
amarincan 0:fb37cd782170 69 break;
amarincan 0:fb37cd782170 70 case 2:
amarincan 0:fb37cd782170 71 lcd.locate(2,1);
amarincan 0:fb37cd782170 72 lcd.printf("= ");
amarincan 0:fb37cd782170 73 lcd.locate(3,1);
amarincan 0:fb37cd782170 74 lcd.printf("%d",m);
amarincan 0:fb37cd782170 75 Ki=m;
amarincan 0:fb37cd782170 76 break;
amarincan 0:fb37cd782170 77 case 3:
amarincan 0:fb37cd782170 78 lcd.locate(10,1);
amarincan 0:fb37cd782170 79 lcd.printf("= ");
amarincan 0:fb37cd782170 80 lcd.locate(11,1);
amarincan 0:fb37cd782170 81 lcd.printf("%d",m);
amarincan 0:fb37cd782170 82 Kd=m;
amarincan 0:fb37cd782170 83 break;
amarincan 0:fb37cd782170 84 }
amarincan 0:fb37cd782170 85 v=m;
amarincan 0:fb37cd782170 86
amarincan 0:fb37cd782170 87 }
amarincan 0:fb37cd782170 88
amarincan 0:fb37cd782170 89
amarincan 0:fb37cd782170 90 if(button3.falling()) {
amarincan 0:fb37cd782170 91 wheel.reset();
amarincan 0:fb37cd782170 92 i++;
amarincan 0:fb37cd782170 93 if(i>3) {
amarincan 0:fb37cd782170 94 i=0;
amarincan 0:fb37cd782170 95 }
amarincan 0:fb37cd782170 96 switch (i) {
amarincan 0:fb37cd782170 97 case 0:
amarincan 0:fb37cd782170 98 lcd.locate(2,0);
amarincan 0:fb37cd782170 99 lcd.printf("=");
amarincan 0:fb37cd782170 100 break;
amarincan 0:fb37cd782170 101 case 1:
amarincan 0:fb37cd782170 102 lcd.locate(10,0);
amarincan 0:fb37cd782170 103 lcd.printf("=");
amarincan 0:fb37cd782170 104 break;
amarincan 0:fb37cd782170 105 case 2:
amarincan 0:fb37cd782170 106 lcd.locate(2,1);
amarincan 0:fb37cd782170 107 lcd.printf("=");
amarincan 0:fb37cd782170 108 break;
amarincan 0:fb37cd782170 109 case 3:
amarincan 0:fb37cd782170 110 lcd.locate(10,1);
amarincan 0:fb37cd782170 111 lcd.printf("=");
amarincan 0:fb37cd782170 112 break;
amarincan 0:fb37cd782170 113 }
amarincan 0:fb37cd782170 114 }
amarincan 0:fb37cd782170 115
amarincan 0:fb37cd782170 116
amarincan 0:fb37cd782170 117 if (button4.falling()){
amarincan 0:fb37cd782170 118 break; //sale del bucle si pisan suiche4
amarincan 0:fb37cd782170 119 }
amarincan 0:fb37cd782170 120
amarincan 0:fb37cd782170 121 }
amarincan 0:fb37cd782170 122 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
amarincan 0:fb37cd782170 123 lcd.cls(); //borra la pantalla
amarincan 0:fb37cd782170 124 lcd.printf(" GUARDADOS!");
amarincan 0:fb37cd782170 125 wait(2);
amarincan 0:fb37cd782170 126 lcd.cls();
amarincan 0:fb37cd782170 127 lcd.printf(" INICIA EL PID");
amarincan 0:fb37cd782170 128 wait(2);
amarincan 0:fb37cd782170 129 // se imprimen los parches del control *****************************************
amarincan 0:fb37cd782170 130 lcd.cls();
amarincan 0:fb37cd782170 131
amarincan 0:fb37cd782170 132
amarincan 0:fb37cd782170 133 lcd.printf("Er%d",err);
amarincan 0:fb37cd782170 134 lcd.locate(8,0);
amarincan 0:fb37cd782170 135 lcd.printf("Me%d",med2);
amarincan 0:fb37cd782170 136 lcd.locate(0,1);
amarincan 0:fb37cd782170 137 lcd.printf("Sp%d",Sp);
amarincan 0:fb37cd782170 138 lcd.locate(8,1);
amarincan 0:fb37cd782170 139 lcd.printf("Pid%d",pid);
amarincan 0:fb37cd782170 140 //wait(5);
amarincan 0:fb37cd782170 141
amarincan 0:fb37cd782170 142 // CICLO PRINCIPAL CONTROLADOR PID
amarincan 0:fb37cd782170 143
amarincan 0:fb37cd782170 144 while(1) {
amarincan 0:fb37cd782170 145 //leer puerto analogo y asignar a med
amarincan 0:fb37cd782170 146 med=Vin.read();
amarincan 0:fb37cd782170 147 med2=med*100;
amarincan 0:fb37cd782170 148 err = (Sp-med2);
amarincan 0:fb37cd782170 149 float kp2;
amarincan 0:fb37cd782170 150 kp2=Kp*0.001;
amarincan 0:fb37cd782170 151 ap = kp2*err;
amarincan 0:fb37cd782170 152 float ki2;
amarincan 0:fb37cd782170 153 ki2=Ki*0.001;
amarincan 0:fb37cd782170 154 ai =(ki2*err)+ai; //calculo de la integral del error
amarincan 0:fb37cd782170 155 float kd2;
amarincan 0:fb37cd782170 156 kd2=Kd*0.0001;
amarincan 0:fb37cd782170 157 ad = kd2*(err-err_v); //calculo de la accion derivativa
amarincan 0:fb37cd782170 158 err_v=err; //guarda el error
amarincan 0:fb37cd782170 159 pid = (ap+ai+ad);
amarincan 0:fb37cd782170 160
amarincan 0:fb37cd782170 161 // se verifica que pid sea menor o igual la valor maximo *****************
amarincan 0:fb37cd782170 162 if (pid > .99999){
amarincan 0:fb37cd782170 163 pid=1;
amarincan 0:fb37cd782170 164 }
amarincan 0:fb37cd782170 165
amarincan 0:fb37cd782170 166 // se verifica que pid sea positivo **************************************
amarincan 0:fb37cd782170 167 if (pid <0){
amarincan 0:fb37cd782170 168 pid=0;
amarincan 0:fb37cd782170 169 }
amarincan 0:fb37cd782170 170
amarincan 0:fb37cd782170 171 //wait(.5);
amarincan 0:fb37cd782170 172
amarincan 0:fb37cd782170 173
amarincan 0:fb37cd782170 174
amarincan 0:fb37cd782170 175 // se verifica que la accion integral no sea muy grande
amarincan 0:fb37cd782170 176 if (ai > 999){
amarincan 0:fb37cd782170 177 ai=1000;
amarincan 0:fb37cd782170 178 }
amarincan 0:fb37cd782170 179
amarincan 0:fb37cd782170 180
amarincan 0:fb37cd782170 181 Vout=pid;
amarincan 0:fb37cd782170 182
amarincan 0:fb37cd782170 183
amarincan 0:fb37cd782170 184 //****se muestran las variables******************************************
amarincan 0:fb37cd782170 185 if(c>600){
amarincan 0:fb37cd782170 186 lcd.locate(2,0);
amarincan 0:fb37cd782170 187 lcd.printf(" ");
amarincan 0:fb37cd782170 188 lcd.locate(0,0);
amarincan 0:fb37cd782170 189 lcd.printf("Er%2.2f",err);
amarincan 0:fb37cd782170 190 lcd.locate(10,0);
amarincan 0:fb37cd782170 191 lcd.printf(" ");
amarincan 0:fb37cd782170 192 lcd.locate(8,0);
amarincan 0:fb37cd782170 193 lcd.printf("Me%d",med2);
amarincan 0:fb37cd782170 194 lcd.locate(2,1);
amarincan 0:fb37cd782170 195 lcd.printf(" ");
amarincan 0:fb37cd782170 196 lcd.locate(0,1);
amarincan 0:fb37cd782170 197 lcd.printf("Sp%d",Sp);
amarincan 0:fb37cd782170 198 lcd.locate(10,1);
amarincan 0:fb37cd782170 199 lcd.printf(" ");
amarincan 0:fb37cd782170 200 lcd.locate(8,1);
amarincan 0:fb37cd782170 201 lcd.printf("Pid%4.3f",pid);
amarincan 0:fb37cd782170 202 c=0;
amarincan 0:fb37cd782170 203 }
amarincan 0:fb37cd782170 204 else
amarincan 0:fb37cd782170 205 c++;
amarincan 0:fb37cd782170 206
amarincan 0:fb37cd782170 207
amarincan 0:fb37cd782170 208
amarincan 0:fb37cd782170 209
amarincan 0:fb37cd782170 210
amarincan 0:fb37cd782170 211
amarincan 0:fb37cd782170 212
amarincan 0:fb37cd782170 213 // se envia el valor pid a puerto analogico de salida (D/A) **************
amarincan 0:fb37cd782170 214 // se repite el ciclo
amarincan 0:fb37cd782170 215 }
amarincan 0:fb37cd782170 216 }