Andrés Mora Bedoya / Mbed 2 deprecated PID_Encoder

Software para realizar control PID en tarjeta Freescale KL25Z con ENCODER y Display 16x2

Dependencies:   DebouncedIna QEI TextLCD1 mbed

/media/uploads/cmorab/foto2.jpg

main.cpp@0:10fa9fae4bf9, 2013-11-20 (annotated)

Committer:
cmorab
Date:
Wed Nov 20 17:33:21 2013 +0000
Revision:
0:10fa9fae4bf9
Software para realizar control PID en tarjeta Freescale KL25Z con ENCODER  y Display 16x2;

Who changed what in which revision?

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