Aldaiir Navarro G / Mbed 2 deprecated T1_PID_AppInv_Encoder

Tarea-1-Procesadores-2016-2

Dependencies:   QEI TextLCD mbed

Fork of PID by Team La Verga

main.cpp@1:e1360b8db5e0, 2016-11-30 (annotated)

Committer:
LaVerga_A
Date:
Wed Nov 30 04:07:18 2016 +0000
Revision:
1:e1360b8db5e0
Parent:
0:8ce5f8fa8f9b 
T1_PID_AppInv_Encoder. Aldaiir Navarro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LaVerga 0:8ce5f8fa8f9b 1 #include "mbed.h"
LaVerga 0:8ce5f8fa8f9b 2 #include "QEI.h"
LaVerga 0:8ce5f8fa8f9b 3 #include "TextLCD.h"
LaVerga 0:8ce5f8fa8f9b 4 #include "stdio.h"
LaVerga 0:8ce5f8fa8f9b 5 #include "string.h"
LaVerga_A 1:e1360b8db5e0 6 #include "stdlib.h"
LaVerga_A 1:e1360b8db5e0 7 #include "math.h"
LaVerga 0:8ce5f8fa8f9b 8
LaVerga_A 1:e1360b8db5e0 9 // Serial pc(USBTX,USBRX); //puertos del PC
LaVerga_A 1:e1360b8db5e0 10 Serial pc(PTE0,PTE1);
LaVerga 0:8ce5f8fa8f9b 11 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
LaVerga 0:8ce5f8fa8f9b 12 QEI encoder (PTA13, PTD5, NC, 624);
LaVerga 0:8ce5f8fa8f9b 13 AnalogIn y(PTB3);//entrada analoga
LaVerga 0:8ce5f8fa8f9b 14 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
LaVerga 0:8ce5f8fa8f9b 15 //si se ignora esto se arruina la FRDMKL25Z
LaVerga_A 1:e1360b8db5e0 16 //DigitalOut led1(LED1);DigitalOut led2(LED2); DigitalOut led3(LED3);
LaVerga 0:8ce5f8fa8f9b 17
LaVerga 0:8ce5f8fa8f9b 18 DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros
LaVerga 0:8ce5f8fa8f9b 19 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
LaVerga 0:8ce5f8fa8f9b 20
LaVerga 0:8ce5f8fa8f9b 21 //int C1=0x0E; // solo muestra el curzor
LaVerga 0:8ce5f8fa8f9b 22 int C2=0x18; // desplaza izquierda
LaVerga 0:8ce5f8fa8f9b 23 int C3=0x1A; // desplaza derecha
LaVerga 0:8ce5f8fa8f9b 24 int C4=0x0C; // quito cursor bajo
LaVerga 0:8ce5f8fa8f9b 25
LaVerga 0:8ce5f8fa8f9b 26 int C1=0x0F;
LaVerga 0:8ce5f8fa8f9b 27 char buffer[6];// TAMAÑO DEL BUFER
LaVerga_A 1:e1360b8db5e0 28 char cadena[3], mm[6];
LaVerga 0:8ce5f8fa8f9b 29 Timer t; //VALOR DEL TIEMPO
LaVerga 0:8ce5f8fa8f9b 30 int count;
LaVerga 0:8ce5f8fa8f9b 31 int i = 0;
LaVerga 0:8ce5f8fa8f9b 32 int c=0;
LaVerga 0:8ce5f8fa8f9b 33 int cambio=0, diferencia=0;
LaVerga 0:8ce5f8fa8f9b 34 float pid,o,ai,ad,ap,med,err;
LaVerga 0:8ce5f8fa8f9b 35 float err_v;
LaVerga 0:8ce5f8fa8f9b 36 int spnum=0,kinum=0,kpnum=0,kdnum=0,num=0,pos=1;
LaVerga_A 1:e1360b8db5e0 37 char bloqueo='d';
LaVerga 0:8ce5f8fa8f9b 38
LaVerga_A 1:e1360b8db5e0 39
LaVerga_A 1:e1360b8db5e0 40 //============================================================================ Function para limpiar Buffer =================
LaVerga_A 1:e1360b8db5e0 41
LaVerga_A 1:e1360b8db5e0 42 int readBuffer(char *buffer,int count)
LaVerga 0:8ce5f8fa8f9b 43 {
LaVerga 0:8ce5f8fa8f9b 44 int i=0;
LaVerga 0:8ce5f8fa8f9b 45 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA
LaVerga 0:8ce5f8fa8f9b 46 while(1) {
LaVerga 0:8ce5f8fa8f9b 47 while (pc.readable()) {
LaVerga 0:8ce5f8fa8f9b 48 char c = pc.getc();
LaVerga 0:8ce5f8fa8f9b 49 //if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracxter inserta $
LaVerga 0:8ce5f8fa8f9b 50 buffer[i++] = c;//mete al bufer el caracter leido
LaVerga 0:8ce5f8fa8f9b 51 if(i > count)break;//sale del loop si ya detecto terminacion
LaVerga 0:8ce5f8fa8f9b 52 }
LaVerga 0:8ce5f8fa8f9b 53 if(i > count)break;
LaVerga 0:8ce5f8fa8f9b 54 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
LaVerga_A 1:e1360b8db5e0 55
LaVerga_A 1:e1360b8db5e0 56 // ======== Analizar aca, tiempo ========================== //
LaVerga 0:8ce5f8fa8f9b 57 t.stop();
LaVerga 0:8ce5f8fa8f9b 58 t.reset();
LaVerga 0:8ce5f8fa8f9b 59 break;
LaVerga 0:8ce5f8fa8f9b 60 }
LaVerga 0:8ce5f8fa8f9b 61 }
LaVerga_A 1:e1360b8db5e0 62 return 0; }
LaVerga 0:8ce5f8fa8f9b 63
LaVerga_A 1:e1360b8db5e0 64 //============================================================================ Function para limpiar Buffer ==============================
LaVerga_A 1:e1360b8db5e0 65 void cleanBuffer(char *buffer, int count)
LaVerga_A 1:e1360b8db5e0 66 {
LaVerga_A 1:e1360b8db5e0 67 for(int i=0; i < count; i++) {
LaVerga_A 1:e1360b8db5e0 68 buffer[i] = '\0'; } }
LaVerga_A 1:e1360b8db5e0 69
LaVerga 0:8ce5f8fa8f9b 70
LaVerga_A 1:e1360b8db5e0 71 //============================================================================ Function Leer Parámetros del Celular ===========================
LaVerga_A 1:e1360b8db5e0 72 void LecturaSerial(void){
LaVerga_A 1:e1360b8db5e0 73 while(bloqueo=='b'){
LaVerga_A 1:e1360b8db5e0 74
LaVerga_A 1:e1360b8db5e0 75 cleanBuffer(buffer,6);
LaVerga_A 1:e1360b8db5e0 76 readBuffer(buffer,6);
LaVerga 0:8ce5f8fa8f9b 77
LaVerga_A 1:e1360b8db5e0 78
LaVerga_A 1:e1360b8db5e0 79 if (buffer[0]=='G'||buffer[0]=='d') { break;}
LaVerga_A 1:e1360b8db5e0 80
LaVerga 0:8ce5f8fa8f9b 81 cadena[0]=buffer[2];
LaVerga 0:8ce5f8fa8f9b 82 cadena[1]=buffer[3];
LaVerga 0:8ce5f8fa8f9b 83 cadena[2]=buffer[4];
LaVerga 0:8ce5f8fa8f9b 84 num=strtod(cadena,NULL);
LaVerga 0:8ce5f8fa8f9b 85
LaVerga_A 1:e1360b8db5e0 86 if(num>999) {num=999; } if(num<0) {num=0; }
LaVerga_A 1:e1360b8db5e0 87
LaVerga_A 1:e1360b8db5e0 88 if (buffer[0]=='S' && buffer[1]=='P')
LaVerga_A 1:e1360b8db5e0 89 { spnum=num;
LaVerga_A 1:e1360b8db5e0 90 lcd.locate(3,0); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 91 lcd.locate(3,0); lcd.printf("%d", spnum);
LaVerga_A 1:e1360b8db5e0 92 pos=1; diferencia=0 ; break; }
LaVerga 0:8ce5f8fa8f9b 93
LaVerga_A 1:e1360b8db5e0 94 if (buffer[0]=='K' && buffer[1]=='P')
LaVerga_A 1:e1360b8db5e0 95 { kpnum=num;
LaVerga_A 1:e1360b8db5e0 96 lcd.locate(11,0); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 97 lcd.locate(11,0); lcd.printf("%d", kpnum);
LaVerga_A 1:e1360b8db5e0 98 pos=2; diferencia=0 ; break;}
LaVerga 0:8ce5f8fa8f9b 99
LaVerga_A 1:e1360b8db5e0 100 if (buffer[0]=='K' && buffer[1]=='I')
LaVerga_A 1:e1360b8db5e0 101 { kinum=num;
LaVerga_A 1:e1360b8db5e0 102 lcd.locate(3,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 103 lcd.locate(3,1); lcd.printf("%d", kinum);
LaVerga_A 1:e1360b8db5e0 104 pos=3; diferencia=0 ; break ;}
LaVerga 0:8ce5f8fa8f9b 105
LaVerga_A 1:e1360b8db5e0 106 if (buffer[0]=='K' && buffer[1]=='D')
LaVerga_A 1:e1360b8db5e0 107 { kdnum=num;
LaVerga_A 1:e1360b8db5e0 108 lcd.locate(11,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 109 lcd.locate(11,1); lcd.printf("%d", kdnum);
LaVerga_A 1:e1360b8db5e0 110 pos=4; diferencia=0 ; break;}
LaVerga_A 1:e1360b8db5e0 111
LaVerga 0:8ce5f8fa8f9b 112
LaVerga_A 1:e1360b8db5e0 113 }//Cierre while(1)
LaVerga_A 1:e1360b8db5e0 114 } // cierra function LecturaSerial
LaVerga_A 1:e1360b8db5e0 115
LaVerga_A 1:e1360b8db5e0 116 //==================================== itoa function ==========================================
LaVerga 0:8ce5f8fa8f9b 117
LaVerga_A 1:e1360b8db5e0 118 /*** C++ version 0.4 char* style "itoa": * Written by Lukás Chmela * Released under GPLv3. */
LaVerga_A 1:e1360b8db5e0 119
LaVerga_A 1:e1360b8db5e0 120 char* itoa(int value, char* result, int base)
LaVerga_A 1:e1360b8db5e0 121 {
LaVerga_A 1:e1360b8db5e0 122 // check that the base if valid
LaVerga_A 1:e1360b8db5e0 123 if ( base < 2 || base > 36 ) {
LaVerga_A 1:e1360b8db5e0 124 *result = '\0';
LaVerga_A 1:e1360b8db5e0 125 return result; }
LaVerga_A 1:e1360b8db5e0 126
LaVerga_A 1:e1360b8db5e0 127 char* ptr = result, *ptr1 = result, tmp_char;
LaVerga_A 1:e1360b8db5e0 128 int tmp_value;
LaVerga_A 1:e1360b8db5e0 129
LaVerga_A 1:e1360b8db5e0 130 do {
LaVerga_A 1:e1360b8db5e0 131 tmp_value = value;
LaVerga_A 1:e1360b8db5e0 132 value /= base;
LaVerga_A 1:e1360b8db5e0 133 *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"[35 + (tmp_value - value * base)];
LaVerga_A 1:e1360b8db5e0 134 } while ( value );
LaVerga_A 1:e1360b8db5e0 135
LaVerga_A 1:e1360b8db5e0 136 // Apply negative sign
LaVerga_A 1:e1360b8db5e0 137 if ( tmp_value < 0 )
LaVerga_A 1:e1360b8db5e0 138 *ptr++ = '-';
LaVerga_A 1:e1360b8db5e0 139 *ptr-- = '\0';
LaVerga_A 1:e1360b8db5e0 140
LaVerga_A 1:e1360b8db5e0 141 while ( ptr1 < ptr ) {
LaVerga_A 1:e1360b8db5e0 142 tmp_char = *ptr;
LaVerga_A 1:e1360b8db5e0 143 *ptr-- = *ptr1;
LaVerga_A 1:e1360b8db5e0 144 *ptr1++ = tmp_char; }
LaVerga_A 1:e1360b8db5e0 145
LaVerga_A 1:e1360b8db5e0 146 return result;
LaVerga_A 1:e1360b8db5e0 147 }
LaVerga 0:8ce5f8fa8f9b 148
LaVerga_A 1:e1360b8db5e0 149 //convertir float a hex
LaVerga_A 1:e1360b8db5e0 150 //=========================================================Float to hex (string)
LaVerga_A 1:e1360b8db5e0 151 char* ftoa(float in,char* out)
LaVerga_A 1:e1360b8db5e0 152
LaVerga_A 1:e1360b8db5e0 153 {
LaVerga_A 1:e1360b8db5e0 154 int a, b;
LaVerga_A 1:e1360b8db5e0 155 char inte[6];
LaVerga_A 1:e1360b8db5e0 156 char flot[6];
LaVerga_A 1:e1360b8db5e0 157
LaVerga_A 1:e1360b8db5e0 158 a=in*1;
LaVerga_A 1:e1360b8db5e0 159 b=(in-a)*10;
LaVerga_A 1:e1360b8db5e0 160
LaVerga_A 1:e1360b8db5e0 161 itoa(a, inte,16);
LaVerga_A 1:e1360b8db5e0 162 itoa(b, flot,16);
LaVerga_A 1:e1360b8db5e0 163
LaVerga_A 1:e1360b8db5e0 164 strcpy (out,inte);
LaVerga_A 1:e1360b8db5e0 165 strcat (out,"2e"); // . ---> 2e ASCII
LaVerga_A 1:e1360b8db5e0 166 strcat (out,flot);
LaVerga_A 1:e1360b8db5e0 167
LaVerga_A 1:e1360b8db5e0 168 return out;
LaVerga_A 1:e1360b8db5e0 169 }
LaVerga_A 1:e1360b8db5e0 170
LaVerga_A 1:e1360b8db5e0 171
LaVerga_A 1:e1360b8db5e0 172 //============================================================================ MAIN ===========================
LaVerga_A 1:e1360b8db5e0 173 int main(void) {
LaVerga 0:8ce5f8fa8f9b 174
LaVerga 0:8ce5f8fa8f9b 175 pc.baud(9600);
LaVerga 0:8ce5f8fa8f9b 176 pc.format(8,Serial::None,1);
LaVerga_A 1:e1360b8db5e0 177
LaVerga 0:8ce5f8fa8f9b 178 lcd.locate(0,1);
LaVerga_A 1:e1360b8db5e0 179 lcd.printf("**Control PID**"); wait(2);
LaVerga_A 1:e1360b8db5e0 180 lcd.cls(); lcd.writeCommand(C1);
LaVerga_A 1:e1360b8db5e0 181
LaVerga_A 1:e1360b8db5e0 182 lcd.locate(8,0); lcd.printf("Kp=%d",kpnum);
LaVerga_A 1:e1360b8db5e0 183 lcd.locate(0,1); lcd.printf("Ki=%d",kinum);
LaVerga_A 1:e1360b8db5e0 184 lcd.locate(8,1); lcd.printf("Kd=%d",kdnum);
LaVerga_A 1:e1360b8db5e0 185 lcd.locate(0,0); lcd.printf("Sp=%d",spnum);
LaVerga 0:8ce5f8fa8f9b 186
LaVerga_A 1:e1360b8db5e0 187 //============================================================================= Lectura del Celular
LaVerga_A 1:e1360b8db5e0 188 while(1){
LaVerga_A 1:e1360b8db5e0 189
LaVerga_A 1:e1360b8db5e0 190 ftoa(509.7,mm) ;
LaVerga_A 1:e1360b8db5e0 191 pc.printf("SP%s", mm);
LaVerga_A 1:e1360b8db5e0 192
LaVerga_A 1:e1360b8db5e0 193
LaVerga_A 1:e1360b8db5e0 194 if(pc.readable())
LaVerga_A 1:e1360b8db5e0 195 { bloqueo='b'; LecturaSerial();
LaVerga_A 1:e1360b8db5e0 196 if (buffer[0]=='G') { break;}}
LaVerga_A 1:e1360b8db5e0 197
LaVerga_A 1:e1360b8db5e0 198 else if(buffer[0]=='S'||buffer[0]=='K'){
LaVerga_A 1:e1360b8db5e0 199 LecturaSerial();}
LaVerga_A 1:e1360b8db5e0 200
LaVerga_A 1:e1360b8db5e0 201 else {bloqueo='d';}
LaVerga_A 1:e1360b8db5e0 202
LaVerga_A 1:e1360b8db5e0 203
LaVerga_A 1:e1360b8db5e0 204 if(!button4){break; } //boton del encoder // wait(0.1);
LaVerga 0:8ce5f8fa8f9b 205
LaVerga 0:8ce5f8fa8f9b 206 diferencia=encoder.getPulses()-cambio;
LaVerga 0:8ce5f8fa8f9b 207 cambio=encoder.getPulses();
LaVerga 0:8ce5f8fa8f9b 208
LaVerga_A 1:e1360b8db5e0 209 if (diferencia==0) { } //nada
LaVerga_A 1:e1360b8db5e0 210
LaVerga 0:8ce5f8fa8f9b 211 else if(diferencia>0)
LaVerga_A 1:e1360b8db5e0 212 { if(pos==1)
LaVerga_A 1:e1360b8db5e0 213 { if(spnum+diferencia>=999)
LaVerga_A 1:e1360b8db5e0 214 { spnum=999;
LaVerga 0:8ce5f8fa8f9b 215 lcd.locate(3,0);
LaVerga 0:8ce5f8fa8f9b 216 lcd.printf(" ");
LaVerga 0:8ce5f8fa8f9b 217 lcd.locate(3,0);
LaVerga_A 1:e1360b8db5e0 218 lcd.printf("%d", spnum); }
LaVerga_A 1:e1360b8db5e0 219 else {
LaVerga_A 1:e1360b8db5e0 220 spnum+=diferencia;
LaVerga 0:8ce5f8fa8f9b 221 lcd.locate(3,0);
LaVerga_A 1:e1360b8db5e0 222 lcd.printf("%d", spnum); }
LaVerga_A 1:e1360b8db5e0 223 //itoa(spnum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 224 //pc.printf("SP%s", mm);
LaVerga_A 1:e1360b8db5e0 225 wait(0.3); } // Adicionado, envio de los parametros al celular
LaVerga_A 1:e1360b8db5e0 226 // convierto numero a caracter y adicional el comodin inicial
LaVerga_A 1:e1360b8db5e0 227
LaVerga 0:8ce5f8fa8f9b 228 else if(pos==2)
LaVerga_A 1:e1360b8db5e0 229 { if(kpnum+diferencia>=999)
LaVerga_A 1:e1360b8db5e0 230 { kpnum=999;
LaVerga_A 1:e1360b8db5e0 231 lcd.locate(11,0);
LaVerga_A 1:e1360b8db5e0 232 lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 233 lcd.locate(11,0);
LaVerga_A 1:e1360b8db5e0 234 lcd.printf("%d", kpnum); }
LaVerga_A 1:e1360b8db5e0 235 else {
LaVerga_A 1:e1360b8db5e0 236 kpnum+=diferencia;
LaVerga_A 1:e1360b8db5e0 237 lcd.locate(11,0);
LaVerga_A 1:e1360b8db5e0 238 lcd.printf("%d", kpnum); }
LaVerga_A 1:e1360b8db5e0 239 // itoa(kpnum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 240 // pc.printf("KP%s", mm);
LaVerga_A 1:e1360b8db5e0 241 wait(0.3); } // https://www.tutorialspoint.com//perl/perl_sprintf.htm
LaVerga_A 1:e1360b8db5e0 242 // http://www.cplusplus.com/reference/cstdlib/itoa/
LaVerga_A 1:e1360b8db5e0 243
LaVerga 0:8ce5f8fa8f9b 244 else if(pos==3)
LaVerga_A 1:e1360b8db5e0 245 { if(kinum+diferencia>=999)
LaVerga_A 1:e1360b8db5e0 246 { kinum=999;
LaVerga_A 1:e1360b8db5e0 247 lcd.locate(3,1);
LaVerga_A 1:e1360b8db5e0 248 lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 249 lcd.locate(3,1);
LaVerga_A 1:e1360b8db5e0 250 lcd.printf("%d", kinum); }
LaVerga 0:8ce5f8fa8f9b 251 else
LaVerga_A 1:e1360b8db5e0 252 { kinum+=diferencia;
LaVerga_A 1:e1360b8db5e0 253 lcd.locate(3,1);
LaVerga_A 1:e1360b8db5e0 254 lcd.printf("%d", kinum); }
LaVerga_A 1:e1360b8db5e0 255 //itoa(kinum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 256 //pc.printf("KI%s", mm);
LaVerga_A 1:e1360b8db5e0 257 wait(0.3); }
LaVerga_A 1:e1360b8db5e0 258
LaVerga 0:8ce5f8fa8f9b 259 else if(pos==4)
LaVerga_A 1:e1360b8db5e0 260 { if(kdnum+diferencia>=999)
LaVerga_A 1:e1360b8db5e0 261 { kdnum=999;
LaVerga_A 1:e1360b8db5e0 262 lcd.locate(11,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 263 lcd.locate(11,1); lcd.printf("%d", kdnum); }
LaVerga 0:8ce5f8fa8f9b 264 else
LaVerga_A 1:e1360b8db5e0 265 { kdnum+=diferencia;
LaVerga_A 1:e1360b8db5e0 266 lcd.locate(11,1); lcd.printf("%d", kdnum); }
LaVerga_A 1:e1360b8db5e0 267 //itoa(kdnum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 268 //pc.printf("KD%s", mm);
LaVerga_A 1:e1360b8db5e0 269 wait(0.3); }
LaVerga_A 1:e1360b8db5e0 270 }
LaVerga 0:8ce5f8fa8f9b 271
LaVerga 0:8ce5f8fa8f9b 272 else if(diferencia<0)
LaVerga 0:8ce5f8fa8f9b 273 {
LaVerga 0:8ce5f8fa8f9b 274 if(pos==1)
LaVerga_A 1:e1360b8db5e0 275 { if(spnum+diferencia<0) { //No ocurre nada
LaVerga 0:8ce5f8fa8f9b 276 }
LaVerga 0:8ce5f8fa8f9b 277 else
LaVerga_A 1:e1360b8db5e0 278 { spnum+=diferencia;
LaVerga_A 1:e1360b8db5e0 279 lcd.locate(3,0); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 280 lcd.locate(3,0); lcd.printf("%d", spnum); }
LaVerga_A 1:e1360b8db5e0 281 itoa(spnum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 282 pc.printf("SP%s", mm);
LaVerga_A 1:e1360b8db5e0 283 wait(0.3); }
LaVerga_A 1:e1360b8db5e0 284
LaVerga_A 1:e1360b8db5e0 285 else if(pos==2)
LaVerga_A 1:e1360b8db5e0 286 { if(kpnum+diferencia<0) { //No ocurre nada
LaVerga 0:8ce5f8fa8f9b 287 }
LaVerga 0:8ce5f8fa8f9b 288 else
LaVerga_A 1:e1360b8db5e0 289 { kpnum+=diferencia;
LaVerga_A 1:e1360b8db5e0 290 lcd.locate(11,0); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 291 lcd.locate(11,0); lcd.printf("%d", kpnum); }
LaVerga_A 1:e1360b8db5e0 292 //itoa(kpnum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 293 //pc.printf("KP%s", mm);
LaVerga_A 1:e1360b8db5e0 294 wait(0.3); }
LaVerga_A 1:e1360b8db5e0 295
LaVerga_A 1:e1360b8db5e0 296 else if(pos==3)
LaVerga_A 1:e1360b8db5e0 297 { if(kinum+diferencia<0) { //No ocurre nada
LaVerga 0:8ce5f8fa8f9b 298 }
LaVerga 0:8ce5f8fa8f9b 299 else
LaVerga_A 1:e1360b8db5e0 300 { kinum+=diferencia;
LaVerga_A 1:e1360b8db5e0 301 lcd.locate(3,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 302 lcd.locate(3,1); lcd.printf("%d", kinum); }
LaVerga_A 1:e1360b8db5e0 303 //itoa(kinum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 304 //pc.printf("KI%s", mm);
LaVerga_A 1:e1360b8db5e0 305 wait(0.3); }
LaVerga_A 1:e1360b8db5e0 306
LaVerga_A 1:e1360b8db5e0 307 else if(pos==4)
LaVerga_A 1:e1360b8db5e0 308 { if(kdnum+diferencia<0)
LaVerga_A 1:e1360b8db5e0 309 { //No ocurre nada
LaVerga 0:8ce5f8fa8f9b 310 }
LaVerga_A 1:e1360b8db5e0 311
LaVerga_A 1:e1360b8db5e0 312 else
LaVerga_A 1:e1360b8db5e0 313 { kdnum+=diferencia;
LaVerga_A 1:e1360b8db5e0 314 lcd.locate(11,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 315 lcd.locate(11,1); lcd.printf("%d", kdnum); }
LaVerga_A 1:e1360b8db5e0 316 //itoa(kdnum,mm,16) ;
LaVerga_A 1:e1360b8db5e0 317 //pc.printf("KD%s", mm);
LaVerga_A 1:e1360b8db5e0 318 wait(0.3); }
LaVerga_A 1:e1360b8db5e0 319 } // cierra el if de cambio en elencoder
LaVerga 0:8ce5f8fa8f9b 320
LaVerga 0:8ce5f8fa8f9b 321
LaVerga_A 1:e1360b8db5e0 322 //===============================================================================
LaVerga_A 1:e1360b8db5e0 323 if (!button3) //cambia la posicion de ingreso de parametros
LaVerga_A 1:e1360b8db5e0 324 { //led3 =!led3;
LaVerga_A 1:e1360b8db5e0 325 if(pos==4)
LaVerga_A 1:e1360b8db5e0 326 { pos=1;
LaVerga_A 1:e1360b8db5e0 327 lcd.locate(3,0); lcd.printf("%d", spnum); }
LaVerga_A 1:e1360b8db5e0 328
LaVerga_A 1:e1360b8db5e0 329 else if (pos==1)
LaVerga_A 1:e1360b8db5e0 330 { pos++;
LaVerga_A 1:e1360b8db5e0 331 lcd.locate(11,0); lcd.printf("%d", kpnum); }
LaVerga_A 1:e1360b8db5e0 332
LaVerga_A 1:e1360b8db5e0 333 else if(pos==2)
LaVerga_A 1:e1360b8db5e0 334 { pos++;
LaVerga_A 1:e1360b8db5e0 335 lcd.locate(3,1); lcd.printf("%d", kinum); }
LaVerga_A 1:e1360b8db5e0 336
LaVerga_A 1:e1360b8db5e0 337 else if(pos==3)
LaVerga_A 1:e1360b8db5e0 338 { pos++;
LaVerga_A 1:e1360b8db5e0 339 lcd.locate(11,1); lcd.printf("%d", kdnum); }
LaVerga_A 1:e1360b8db5e0 340
LaVerga_A 1:e1360b8db5e0 341 wait(0.25);
LaVerga_A 1:e1360b8db5e0 342 }
LaVerga_A 1:e1360b8db5e0 343 } // cierra el while 1
LaVerga_A 1:e1360b8db5e0 344
LaVerga 0:8ce5f8fa8f9b 345
LaVerga_A 1:e1360b8db5e0 346 //==============================================================================Transición hacia el PID
LaVerga_A 1:e1360b8db5e0 347 lcd.writeCommand(C4); //quitar cursor
LaVerga_A 1:e1360b8db5e0 348 lcd.cls(); lcd.printf(" GUARDADOS!"); wait(1);
LaVerga_A 1:e1360b8db5e0 349 lcd.cls(); lcd.printf(" INICIA EL PID"); wait(1);
LaVerga_A 1:e1360b8db5e0 350
LaVerga 0:8ce5f8fa8f9b 351 // se imprimen los parches del control *****************************************
LaVerga_A 1:e1360b8db5e0 352 lcd.cls(); lcd.printf("Er=%3.0f",err);
LaVerga_A 1:e1360b8db5e0 353 lcd.locate(8,0); lcd.printf("Me=%3.0f",med);
LaVerga_A 1:e1360b8db5e0 354 lcd.locate(0,1); lcd.printf("Sp=%3.0f",spnum);
LaVerga_A 1:e1360b8db5e0 355 lcd.locate(8,1); lcd.printf("Co=%3.0f",pid);
LaVerga 0:8ce5f8fa8f9b 356 wait(1);
LaVerga 0:8ce5f8fa8f9b 357
LaVerga 0:8ce5f8fa8f9b 358 // CICLO PRINCIPAL CONTROLADOR PID
LaVerga 0:8ce5f8fa8f9b 359 lop1: med = y.read()*999;
LaVerga 0:8ce5f8fa8f9b 360 err = (spnum-med); //se calcula el error
LaVerga 0:8ce5f8fa8f9b 361 ap = kpnum*err*0.01f; //se calcula la accion proporcinal
LaVerga 0:8ce5f8fa8f9b 362 ai =(kinum*err*0.01f)+ai; //calculo de la integral del error
LaVerga 0:8ce5f8fa8f9b 363 ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
LaVerga 0:8ce5f8fa8f9b 364 pid = (ap+ai+ad);
LaVerga_A 1:e1360b8db5e0 365
LaVerga_A 1:e1360b8db5e0 366 if(pid<=0) { // se verifica que pid sea positivo **************************************
LaVerga_A 1:e1360b8db5e0 367 pid=0; }
LaVerga 0:8ce5f8fa8f9b 368
LaVerga_A 1:e1360b8db5e0 369
LaVerga_A 1:e1360b8db5e0 370 if (pid > 999) { // se verifica que pid sea menor o igual la valor maximo *****************
LaVerga_A 1:e1360b8db5e0 371 pid=999; }
LaVerga 0:8ce5f8fa8f9b 372
LaVerga_A 1:e1360b8db5e0 373 //se muestran Y ENVIAN las variables******************************************
LaVerga_A 1:e1360b8db5e0 374 lcd.locate(3,0); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 375 lcd.locate(3,0); lcd.printf("%3.0f",err);
LaVerga_A 1:e1360b8db5e0 376 ftoa(err,mm) ; pc.printf("ER%s", mm);
LaVerga_A 1:e1360b8db5e0 377 wait(0.3);
LaVerga 0:8ce5f8fa8f9b 378
LaVerga_A 1:e1360b8db5e0 379 lcd.locate(11,0); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 380 lcd.locate(11,0); lcd.printf("%3.0f",med);
LaVerga_A 1:e1360b8db5e0 381 ftoa(med,mm) ; pc.printf("ME%s", mm);
LaVerga_A 1:e1360b8db5e0 382 wait(0.3);
LaVerga_A 1:e1360b8db5e0 383
LaVerga_A 1:e1360b8db5e0 384 lcd.locate(3,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 385 lcd.locate(3,1); lcd.printf("%d",spnum);
LaVerga_A 1:e1360b8db5e0 386
LaVerga_A 1:e1360b8db5e0 387 lcd.locate(11,1); lcd.printf(" ");
LaVerga_A 1:e1360b8db5e0 388 lcd.locate(11,1); lcd.printf("%3.0f",pid);
LaVerga_A 1:e1360b8db5e0 389 ftoa(pid,mm) ; pc.printf("ID%s", mm);
LaVerga_A 1:e1360b8db5e0 390 wait(0.3);
LaVerga_A 1:e1360b8db5e0 391
LaVerga 0:8ce5f8fa8f9b 392 //Normalizacion de la salida
LaVerga 0:8ce5f8fa8f9b 393 // se actualizan las variables *******************************************
LaVerga 0:8ce5f8fa8f9b 394 err_v = err;
LaVerga 0:8ce5f8fa8f9b 395 o = pid/999;
LaVerga 0:8ce5f8fa8f9b 396 u.write(o);
LaVerga 0:8ce5f8fa8f9b 397 // se envia el valor pid a puerto analogico de salida (D/A) **************
LaVerga_A 1:e1360b8db5e0 398 wait_ms(300);
LaVerga 0:8ce5f8fa8f9b 399
LaVerga_A 1:e1360b8db5e0 400 goto lop1; // se repite el ciclo
LaVerga 0:8ce5f8fa8f9b 401
LaVerga 0:8ce5f8fa8f9b 402 }//Cierre main