Andrés López / Mbed 2 deprecated PID_BLUETOOT_mod

PID programable a traves de un modulo bluetooth

Dependencies:   BufferedSerial QEI TextLCD mbed

main.cpp@0:acd6b856d63c, 2016-11-30 (annotated)

Committer:
andJdmat
Date:
Wed Nov 30 02:00:39 2016 +0000
Revision:
0:acd6b856d63c
PID programable a traves de bluetooth;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andJdmat 0:acd6b856d63c 1 #include "mbed.h"
andJdmat 0:acd6b856d63c 2 #include "QEI.h"
andJdmat 0:acd6b856d63c 3 #include "TextLCD.h"
andJdmat 0:acd6b856d63c 4 #include "iostream"
andJdmat 0:acd6b856d63c 5 #include "stdio.h"
andJdmat 0:acd6b856d63c 6 #include "string"
andJdmat 0:acd6b856d63c 7 #include "stdlib.h"
andJdmat 0:acd6b856d63c 8 #include <BufferedSerial.h>
andJdmat 0:acd6b856d63c 9
andJdmat 0:acd6b856d63c 10 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5, TextLCD::LCD20x4); // rs, e, d4-d7 Teclado
andJdmat 0:acd6b856d63c 11 //asignamos el puerto a cada interruptor
andJdmat 0:acd6b856d63c 12 QEI encoder (PTA13, PTD5, NC, 624, QEI::X4_ENCODING);
andJdmat 0:acd6b856d63c 13 Serial GSM(PTE0,PTE1);
andJdmat 0:acd6b856d63c 14 AnalogIn y(PTB3);//entrada analoga
andJdmat 0:acd6b856d63c 15 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
andJdmat 0:acd6b856d63c 16 //si se ignora esto se arruina la FRDMKL25Z
andJdmat 0:acd6b856d63c 17 DigitalOut led1(LED1);
andJdmat 0:acd6b856d63c 18 DigitalOut led2(LED2);
andJdmat 0:acd6b856d63c 19 DigitalOut led3(LED3);
andJdmat 0:acd6b856d63c 20
andJdmat 0:acd6b856d63c 21 DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros
andJdmat 0:acd6b856d63c 22 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
andJdmat 0:acd6b856d63c 23
andJdmat 0:acd6b856d63c 24 Serial pc(USBTX, USBRX); // tx, rx
andJdmat 0:acd6b856d63c 25 Serial device(PTE0, PTE1); // tx, rx
andJdmat 0:acd6b856d63c 26 //device.baud(115200);
andJdmat 0:acd6b856d63c 27
andJdmat 0:acd6b856d63c 28
andJdmat 0:acd6b856d63c 29 //codigos movimiento del curzor
andJdmat 0:acd6b856d63c 30
andJdmat 0:acd6b856d63c 31 //int C1=0x0E; // solo muestra el curzor
andJdmat 0:acd6b856d63c 32 int C2=0x18; // desplaza izquierda
andJdmat 0:acd6b856d63c 33 int C3=0x1A; // desplaza derecha
andJdmat 0:acd6b856d63c 34 int C4=0x0C; // quito cursor bajo
andJdmat 0:acd6b856d63c 35
andJdmat 0:acd6b856d63c 36 int C1=0x0F;
andJdmat 0:acd6b856d63c 37 int cambio=0, diferencia=0;
andJdmat 0:acd6b856d63c 38 // se cambio de float a entero
andJdmat 0:acd6b856d63c 39
andJdmat 0:acd6b856d63c 40 float pid,o,ai,ad,ap,med,err;
andJdmat 0:acd6b856d63c 41 float err_v;
andJdmat 0:acd6b856d63c 42
andJdmat 0:acd6b856d63c 43 // Valores de k y de j
andJdmat 0:acd6b856d63c 44 int j=0;
andJdmat 0:acd6b856d63c 45 int k=0;
andJdmat 0:acd6b856d63c 46
andJdmat 0:acd6b856d63c 47 // fin del cambio
andJdmat 0:acd6b856d63c 48 int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
andJdmat 0:acd6b856d63c 49 char buffer[128];
andJdmat 0:acd6b856d63c 50 char buffer2[128];
andJdmat 0:acd6b856d63c 51 char salidas[32];
andJdmat 0:acd6b856d63c 52 char err_s[3];
andJdmat 0:acd6b856d63c 53 char spnum_s[3];
andJdmat 0:acd6b856d63c 54 char med_s[3];
andJdmat 0:acd6b856d63c 55 char co_s[3];
andJdmat 0:acd6b856d63c 56 Timer t;
andJdmat 0:acd6b856d63c 57 int l;
andJdmat 0:acd6b856d63c 58
andJdmat 0:acd6b856d63c 59
andJdmat 0:acd6b856d63c 60 // Inicio de modificaciones, en esta parte se agregaron funciones que permiten imprimir datos en la lcd que envia
andJdmat 0:acd6b856d63c 61 // el modulo bluetooth, el codigo fue depurado usando la comunicacion serial.
andJdmat 0:acd6b856d63c 62 char* itoa(int value, char* result, int base) // Funcion para convertir enteros a cadenas de texto, fue importada de un foro
andJdmat 0:acd6b856d63c 63 {
andJdmat 0:acd6b856d63c 64 // check that the base if valid
andJdmat 0:acd6b856d63c 65 if ( base < 2 || base > 36 ) {
andJdmat 0:acd6b856d63c 66 *result = '\0';
andJdmat 0:acd6b856d63c 67 return result;
andJdmat 0:acd6b856d63c 68 }
andJdmat 0:acd6b856d63c 69
andJdmat 0:acd6b856d63c 70 char* ptr = result, *ptr1 = result, tmp_char;
andJdmat 0:acd6b856d63c 71 int tmp_value;
andJdmat 0:acd6b856d63c 72
andJdmat 0:acd6b856d63c 73 do {
andJdmat 0:acd6b856d63c 74 tmp_value = value;
andJdmat 0:acd6b856d63c 75 value /= base;
andJdmat 0:acd6b856d63c 76 *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"[35 + (tmp_value - value * base)];
andJdmat 0:acd6b856d63c 77 } while ( value );
andJdmat 0:acd6b856d63c 78
andJdmat 0:acd6b856d63c 79 // Apply negative sign
andJdmat 0:acd6b856d63c 80 if ( tmp_value < 0 )
andJdmat 0:acd6b856d63c 81 *ptr++ = '-';
andJdmat 0:acd6b856d63c 82 *ptr-- = '\0';
andJdmat 0:acd6b856d63c 83
andJdmat 0:acd6b856d63c 84 while ( ptr1 < ptr ) {
andJdmat 0:acd6b856d63c 85 tmp_char = *ptr;
andJdmat 0:acd6b856d63c 86 *ptr-- = *ptr1;
andJdmat 0:acd6b856d63c 87 *ptr1++ = tmp_char;
andJdmat 0:acd6b856d63c 88 }
andJdmat 0:acd6b856d63c 89
andJdmat 0:acd6b856d63c 90 return result;
andJdmat 0:acd6b856d63c 91 }
andJdmat 0:acd6b856d63c 92
andJdmat 0:acd6b856d63c 93
andJdmat 0:acd6b856d63c 94 int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos
andJdmat 0:acd6b856d63c 95 {
andJdmat 0:acd6b856d63c 96 int i=0;
andJdmat 0:acd6b856d63c 97 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA
andJdmat 0:acd6b856d63c 98 while(1) {
andJdmat 0:acd6b856d63c 99 while (device.readable()) {
andJdmat 0:acd6b856d63c 100 char c = device.getc();
andJdmat 0:acd6b856d63c 101 //if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracxter inserta $
andJdmat 0:acd6b856d63c 102 buffer[i++ - 1] = c;//mete al bufer el caracter leido
andJdmat 0:acd6b856d63c 103 if(i > count)break;//sale del loop si ya detecto terminacion
andJdmat 0:acd6b856d63c 104 }
andJdmat 0:acd6b856d63c 105 if(i > count)break;
andJdmat 0:acd6b856d63c 106 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
andJdmat 0:acd6b856d63c 107 t.stop();
andJdmat 0:acd6b856d63c 108 t.reset();
andJdmat 0:acd6b856d63c 109 break;
andJdmat 0:acd6b856d63c 110 }
andJdmat 0:acd6b856d63c 111 }
andJdmat 0:acd6b856d63c 112 return 0;
andJdmat 0:acd6b856d63c 113 }
andJdmat 0:acd6b856d63c 114
andJdmat 0:acd6b856d63c 115 void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer
andJdmat 0:acd6b856d63c 116 {
andJdmat 0:acd6b856d63c 117 for(int i=0; i < count; i++) {
andJdmat 0:acd6b856d63c 118 buffer[i] = '\0';
andJdmat 0:acd6b856d63c 119 }
andJdmat 0:acd6b856d63c 120 }
andJdmat 0:acd6b856d63c 121
andJdmat 0:acd6b856d63c 122 int tamano(char *buffer)
andJdmat 0:acd6b856d63c 123 {
andJdmat 0:acd6b856d63c 124 int tam = 0;
andJdmat 0:acd6b856d63c 125 while (buffer[tam] != '\0')
andJdmat 0:acd6b856d63c 126 {
andJdmat 0:acd6b856d63c 127 tam++;
andJdmat 0:acd6b856d63c 128 }
andJdmat 0:acd6b856d63c 129 return tam;
andJdmat 0:acd6b856d63c 130 }
andJdmat 0:acd6b856d63c 131
andJdmat 0:acd6b856d63c 132 void verificacion(char *buffer) // Esta funcion fue creada para esta aplicacion especifica, permite verificar si los datos que entraron a la freescale
andJdmat 0:acd6b856d63c 133 // son los esperados y adicionalmente crea un nuevo buffer con los datos para imprimir.
andJdmat 0:acd6b856d63c 134 {
andJdmat 0:acd6b856d63c 135 int l = tamano(buffer);
andJdmat 0:acd6b856d63c 136 buffer2[0] = '-';
andJdmat 0:acd6b856d63c 137 if (l < 16)
andJdmat 0:acd6b856d63c 138 {
andJdmat 0:acd6b856d63c 139 if(buffer[0]=='\0')
andJdmat 0:acd6b856d63c 140 {
andJdmat 0:acd6b856d63c 141 for (int i = 0; i < 16; i++)
andJdmat 0:acd6b856d63c 142 {
andJdmat 0:acd6b856d63c 143 buffer2[i] = '0';
andJdmat 0:acd6b856d63c 144 }
andJdmat 0:acd6b856d63c 145 }
andJdmat 0:acd6b856d63c 146 else
andJdmat 0:acd6b856d63c 147 {
andJdmat 0:acd6b856d63c 148 int k = 0;
andJdmat 0:acd6b856d63c 149 int j = 0;
andJdmat 0:acd6b856d63c 150 for(int i=0; i < 15; i++)
andJdmat 0:acd6b856d63c 151 {
andJdmat 0:acd6b856d63c 152 if(j == 1)
andJdmat 0:acd6b856d63c 153 {
andJdmat 0:acd6b856d63c 154 if(buffer[i+1]=='-')
andJdmat 0:acd6b856d63c 155 {
andJdmat 0:acd6b856d63c 156 j = 0;
andJdmat 0:acd6b856d63c 157 }
andJdmat 0:acd6b856d63c 158 else if (buffer[i+2]=='-')
andJdmat 0:acd6b856d63c 159 {
andJdmat 0:acd6b856d63c 160 j = 0;
andJdmat 0:acd6b856d63c 161 i = i+1;
andJdmat 0:acd6b856d63c 162 }
andJdmat 0:acd6b856d63c 163 else if (buffer[i+3]=='-')
andJdmat 0:acd6b856d63c 164 {
andJdmat 0:acd6b856d63c 165 j = 0;
andJdmat 0:acd6b856d63c 166 i = i+2;
andJdmat 0:acd6b856d63c 167 }
andJdmat 0:acd6b856d63c 168 else if(buffer[i+4]=='-')
andJdmat 0:acd6b856d63c 169 {
andJdmat 0:acd6b856d63c 170 j = 0;
andJdmat 0:acd6b856d63c 171 i = i+3;
andJdmat 0:acd6b856d63c 172 }
andJdmat 0:acd6b856d63c 173 }
andJdmat 0:acd6b856d63c 174
andJdmat 0:acd6b856d63c 175 else
andJdmat 0:acd6b856d63c 176 {
andJdmat 0:acd6b856d63c 177 if(buffer[i+1]=='-')
andJdmat 0:acd6b856d63c 178 {
andJdmat 0:acd6b856d63c 179 buffer2[k + 1] = '0';
andJdmat 0:acd6b856d63c 180 buffer2[k + 2] = '0';
andJdmat 0:acd6b856d63c 181 buffer2[k + 3] = '0';
andJdmat 0:acd6b856d63c 182 buffer2[k + 4] = '-';
andJdmat 0:acd6b856d63c 183 k = k + 4;
andJdmat 0:acd6b856d63c 184 j = 1;
andJdmat 0:acd6b856d63c 185 }
andJdmat 0:acd6b856d63c 186 else if (buffer[i+2]=='-')
andJdmat 0:acd6b856d63c 187 {
andJdmat 0:acd6b856d63c 188 buffer2[k + 1] = '0';
andJdmat 0:acd6b856d63c 189 buffer2[k + 2] = '0';
andJdmat 0:acd6b856d63c 190 buffer2[k + 3] = buffer[i+1];
andJdmat 0:acd6b856d63c 191 buffer2[k + 4] = '-';
andJdmat 0:acd6b856d63c 192 k = k + 4;
andJdmat 0:acd6b856d63c 193 j = 1;
andJdmat 0:acd6b856d63c 194 }
andJdmat 0:acd6b856d63c 195 else if (buffer[i+3]=='-')
andJdmat 0:acd6b856d63c 196 {
andJdmat 0:acd6b856d63c 197 buffer2[k + 1] = '0';
andJdmat 0:acd6b856d63c 198 buffer2[k + 2] = buffer[i+1];
andJdmat 0:acd6b856d63c 199 buffer2[k + 3] = buffer[i+2];
andJdmat 0:acd6b856d63c 200 buffer2[k + 4] = '-';
andJdmat 0:acd6b856d63c 201 k = k+4;
andJdmat 0:acd6b856d63c 202 j = 1;
andJdmat 0:acd6b856d63c 203 }
andJdmat 0:acd6b856d63c 204 else if(buffer[i+4]=='-')
andJdmat 0:acd6b856d63c 205 {
andJdmat 0:acd6b856d63c 206 buffer2[k + 1] = buffer[i+1];
andJdmat 0:acd6b856d63c 207 buffer2[k + 2] = buffer[i+2];
andJdmat 0:acd6b856d63c 208 buffer2[k + 3] = buffer[i+3];
andJdmat 0:acd6b856d63c 209 buffer2[k + 4] = '-';
andJdmat 0:acd6b856d63c 210 k = k+4;
andJdmat 0:acd6b856d63c 211 j =1;
andJdmat 0:acd6b856d63c 212 }
andJdmat 0:acd6b856d63c 213 }
andJdmat 0:acd6b856d63c 214 }
andJdmat 0:acd6b856d63c 215 }
andJdmat 0:acd6b856d63c 216 }
andJdmat 0:acd6b856d63c 217 else
andJdmat 0:acd6b856d63c 218 {
andJdmat 0:acd6b856d63c 219 for (int i = 0; i < 16; i++)
andJdmat 0:acd6b856d63c 220 {
andJdmat 0:acd6b856d63c 221 buffer2[i] = buffer[i];
andJdmat 0:acd6b856d63c 222 }
andJdmat 0:acd6b856d63c 223 }
andJdmat 0:acd6b856d63c 224 }
andJdmat 0:acd6b856d63c 225 // Fin de la modificacion para la impresion por bluetooth
andJdmat 0:acd6b856d63c 226 int main()
andJdmat 0:acd6b856d63c 227 {
andJdmat 0:acd6b856d63c 228 lcd.locate(0,1);
andJdmat 0:acd6b856d63c 229 lcd.printf("**Control PID**");
andJdmat 0:acd6b856d63c 230 wait(2);
andJdmat 0:acd6b856d63c 231 lcd.cls(); // Borrar Pantalla
andJdmat 0:acd6b856d63c 232 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
andJdmat 0:acd6b856d63c 233
andJdmat 0:acd6b856d63c 234 lcd.locate(8,0);
andJdmat 0:acd6b856d63c 235 lcd.printf("Kp=%d",kpnum);
andJdmat 0:acd6b856d63c 236 lcd.locate(0,1);
andJdmat 0:acd6b856d63c 237 lcd.printf("Ki=%d",kinum);
andJdmat 0:acd6b856d63c 238 lcd.locate(8,1);
andJdmat 0:acd6b856d63c 239 lcd.printf("Kd=%d",kdnum);
andJdmat 0:acd6b856d63c 240 lcd.locate(0,0);
andJdmat 0:acd6b856d63c 241 lcd.printf("Sp=%d",spnum);
andJdmat 0:acd6b856d63c 242
andJdmat 0:acd6b856d63c 243 while(1)
andJdmat 0:acd6b856d63c 244 {
andJdmat 0:acd6b856d63c 245 //lcd.locate(8,0);
andJdmat 0:acd6b856d63c 246 //lcd.printf("Kp=%d",encoder.getPulses());
andJdmat 0:acd6b856d63c 247 //wait(.5);
andJdmat 0:acd6b856d63c 248
andJdmat 0:acd6b856d63c 249 if(device.readable()) // Se comprueba si ha datos para leer y se imprimen.
andJdmat 0:acd6b856d63c 250 {
andJdmat 0:acd6b856d63c 251
andJdmat 0:acd6b856d63c 252 cleanBuffer(buffer2,128);
andJdmat 0:acd6b856d63c 253 cleanBuffer(buffer,128);
andJdmat 0:acd6b856d63c 254 readBuffer(buffer,128);
andJdmat 0:acd6b856d63c 255 if (buffer[0] == 'p'){break;}
andJdmat 0:acd6b856d63c 256 pc.printf("buffer= %s\n\r ",buffer); //imprime el bufer
andJdmat 0:acd6b856d63c 257 verificacion(buffer);
andJdmat 0:acd6b856d63c 258 pc.printf("buffer2= %s\n\r ",buffer2); //imprime el buffer reconstruido
andJdmat 0:acd6b856d63c 259 pc.printf("tamano buffer= %d \n\r",l); //imprime el tamaño
andJdmat 0:acd6b856d63c 260 pc.printf("Sp= %c %c %c\n\r ",buffer2[1],buffer2[2],buffer2[3]);
andJdmat 0:acd6b856d63c 261 pc.printf("Kp= %c %c %c\n\r ",buffer2[5],buffer2[6],buffer2[7]);
andJdmat 0:acd6b856d63c 262 pc.printf("Ki= %c %c %c\n\r ",buffer2[9],buffer2[10],buffer2[11]);
andJdmat 0:acd6b856d63c 263 pc.printf("Kd= %c %c %c\n\r ",buffer2[13],buffer2[14],buffer2[15]);
andJdmat 0:acd6b856d63c 264 spnum = 1*(buffer2[3]-48)+ 10*(buffer2[2]-48)+ 100*(buffer2[1]-48);
andJdmat 0:acd6b856d63c 265
andJdmat 0:acd6b856d63c 266 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 267 lcd.printf(" ");
andJdmat 0:acd6b856d63c 268 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 269 lcd.printf("%d", spnum);
andJdmat 0:acd6b856d63c 270 kpnum = 1*(buffer2[7]-48)+ 10*(buffer2[6]-48)+ 100*(buffer2[5]-48);
andJdmat 0:acd6b856d63c 271 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 272 lcd.printf(" ");
andJdmat 0:acd6b856d63c 273 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 274 lcd.printf("%d", kpnum);
andJdmat 0:acd6b856d63c 275 kinum = 1*(buffer2[11]-48)+ 10*(buffer2[10]-48)+ 100*(buffer2[9]-48);
andJdmat 0:acd6b856d63c 276 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 277 lcd.printf(" ");
andJdmat 0:acd6b856d63c 278 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 279 lcd.printf("%d", kinum);
andJdmat 0:acd6b856d63c 280 kdnum = 1*(buffer2[15]-48)+ 10*(buffer2[14]-48)+ 100*(buffer2[13]-48);
andJdmat 0:acd6b856d63c 281 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 282 lcd.printf(" ");
andJdmat 0:acd6b856d63c 283 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 284 lcd.printf("%d", kdnum);
andJdmat 0:acd6b856d63c 285 cleanBuffer(buffer2,128);
andJdmat 0:acd6b856d63c 286 cleanBuffer(buffer,128);
andJdmat 0:acd6b856d63c 287 }
andJdmat 0:acd6b856d63c 288 else{
andJdmat 0:acd6b856d63c 289
andJdmat 0:acd6b856d63c 290 diferencia=encoder.getPulses()-cambio;
andJdmat 0:acd6b856d63c 291 cambio=encoder.getPulses();
andJdmat 0:acd6b856d63c 292
andJdmat 0:acd6b856d63c 293 if (diferencia==0)
andJdmat 0:acd6b856d63c 294 {
andJdmat 0:acd6b856d63c 295 //nada
andJdmat 0:acd6b856d63c 296 }
andJdmat 0:acd6b856d63c 297 else if(diferencia>0)
andJdmat 0:acd6b856d63c 298 {
andJdmat 0:acd6b856d63c 299 if(pos==1)
andJdmat 0:acd6b856d63c 300 {
andJdmat 0:acd6b856d63c 301 if(spnum+diferencia>=999)
andJdmat 0:acd6b856d63c 302 {
andJdmat 0:acd6b856d63c 303 spnum=999;
andJdmat 0:acd6b856d63c 304 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 305 lcd.printf(" ");
andJdmat 0:acd6b856d63c 306 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 307 lcd.printf("%d", spnum);
andJdmat 0:acd6b856d63c 308 }
andJdmat 0:acd6b856d63c 309 else
andJdmat 0:acd6b856d63c 310 {
andJdmat 0:acd6b856d63c 311 spnum+=diferencia;
andJdmat 0:acd6b856d63c 312 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 313 lcd.printf("%d", spnum);
andJdmat 0:acd6b856d63c 314 }
andJdmat 0:acd6b856d63c 315 }
andJdmat 0:acd6b856d63c 316 else if(pos==2)
andJdmat 0:acd6b856d63c 317 {
andJdmat 0:acd6b856d63c 318 if(kpnum+diferencia>=999)
andJdmat 0:acd6b856d63c 319 {
andJdmat 0:acd6b856d63c 320 kpnum=999;
andJdmat 0:acd6b856d63c 321 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 322 lcd.printf(" ");
andJdmat 0:acd6b856d63c 323 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 324 lcd.printf("%d", kpnum);
andJdmat 0:acd6b856d63c 325 }
andJdmat 0:acd6b856d63c 326 else
andJdmat 0:acd6b856d63c 327 {
andJdmat 0:acd6b856d63c 328 kpnum+=diferencia;
andJdmat 0:acd6b856d63c 329 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 330 lcd.printf("%d", kpnum);
andJdmat 0:acd6b856d63c 331 }
andJdmat 0:acd6b856d63c 332 }
andJdmat 0:acd6b856d63c 333 else if(pos==3)
andJdmat 0:acd6b856d63c 334 {
andJdmat 0:acd6b856d63c 335 if(kinum+diferencia>=999)
andJdmat 0:acd6b856d63c 336 {
andJdmat 0:acd6b856d63c 337 kinum=999;
andJdmat 0:acd6b856d63c 338 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 339 lcd.printf(" ");
andJdmat 0:acd6b856d63c 340 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 341 lcd.printf("%d", kinum);
andJdmat 0:acd6b856d63c 342 }
andJdmat 0:acd6b856d63c 343 else
andJdmat 0:acd6b856d63c 344 {
andJdmat 0:acd6b856d63c 345 kinum+=diferencia;
andJdmat 0:acd6b856d63c 346 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 347 lcd.printf("%d", kinum);
andJdmat 0:acd6b856d63c 348 }
andJdmat 0:acd6b856d63c 349 }
andJdmat 0:acd6b856d63c 350 else if(pos==4)
andJdmat 0:acd6b856d63c 351 {
andJdmat 0:acd6b856d63c 352 if(kdnum+diferencia>=999)
andJdmat 0:acd6b856d63c 353 {
andJdmat 0:acd6b856d63c 354 kdnum=999;
andJdmat 0:acd6b856d63c 355 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 356 lcd.printf(" ");
andJdmat 0:acd6b856d63c 357 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 358 lcd.printf("%d", kdnum);
andJdmat 0:acd6b856d63c 359 }
andJdmat 0:acd6b856d63c 360 else
andJdmat 0:acd6b856d63c 361 {
andJdmat 0:acd6b856d63c 362 kdnum+=diferencia;
andJdmat 0:acd6b856d63c 363 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 364 lcd.printf("%d", kdnum);
andJdmat 0:acd6b856d63c 365 }
andJdmat 0:acd6b856d63c 366 }
andJdmat 0:acd6b856d63c 367 }
andJdmat 0:acd6b856d63c 368
andJdmat 0:acd6b856d63c 369 else if(diferencia<0)
andJdmat 0:acd6b856d63c 370 {
andJdmat 0:acd6b856d63c 371 if(pos==1)
andJdmat 0:acd6b856d63c 372 {
andJdmat 0:acd6b856d63c 373 if(spnum+diferencia<0)
andJdmat 0:acd6b856d63c 374 {
andJdmat 0:acd6b856d63c 375 //No ocurre nada
andJdmat 0:acd6b856d63c 376 }
andJdmat 0:acd6b856d63c 377 else
andJdmat 0:acd6b856d63c 378 {
andJdmat 0:acd6b856d63c 379 spnum+=diferencia;
andJdmat 0:acd6b856d63c 380 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 381 lcd.printf(" ");
andJdmat 0:acd6b856d63c 382 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 383 lcd.printf("%d", spnum);
andJdmat 0:acd6b856d63c 384 }
andJdmat 0:acd6b856d63c 385 }
andJdmat 0:acd6b856d63c 386 else if(pos==2)
andJdmat 0:acd6b856d63c 387 {
andJdmat 0:acd6b856d63c 388 if(kpnum+diferencia<0)
andJdmat 0:acd6b856d63c 389 {
andJdmat 0:acd6b856d63c 390 //No ocurre nada
andJdmat 0:acd6b856d63c 391 }
andJdmat 0:acd6b856d63c 392 else
andJdmat 0:acd6b856d63c 393 {
andJdmat 0:acd6b856d63c 394 kpnum+=diferencia;
andJdmat 0:acd6b856d63c 395 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 396 lcd.printf(" ");
andJdmat 0:acd6b856d63c 397 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 398 lcd.printf("%d", kpnum);
andJdmat 0:acd6b856d63c 399 }
andJdmat 0:acd6b856d63c 400 }
andJdmat 0:acd6b856d63c 401 else if(pos==3)
andJdmat 0:acd6b856d63c 402 {
andJdmat 0:acd6b856d63c 403 if(kinum+diferencia<0)
andJdmat 0:acd6b856d63c 404 {
andJdmat 0:acd6b856d63c 405 //No ocurre nada
andJdmat 0:acd6b856d63c 406 }
andJdmat 0:acd6b856d63c 407 else
andJdmat 0:acd6b856d63c 408 {
andJdmat 0:acd6b856d63c 409 kinum+=diferencia;
andJdmat 0:acd6b856d63c 410 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 411 lcd.printf(" ");
andJdmat 0:acd6b856d63c 412 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 413 lcd.printf("%d", kinum);
andJdmat 0:acd6b856d63c 414 }
andJdmat 0:acd6b856d63c 415 }
andJdmat 0:acd6b856d63c 416 else if(pos==4)
andJdmat 0:acd6b856d63c 417 {
andJdmat 0:acd6b856d63c 418 if(kdnum+diferencia<0)
andJdmat 0:acd6b856d63c 419 {
andJdmat 0:acd6b856d63c 420 //No ocurre nada
andJdmat 0:acd6b856d63c 421 }
andJdmat 0:acd6b856d63c 422 else
andJdmat 0:acd6b856d63c 423 {
andJdmat 0:acd6b856d63c 424 kdnum+=diferencia;
andJdmat 0:acd6b856d63c 425 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 426 lcd.printf(" ");
andJdmat 0:acd6b856d63c 427 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 428 lcd.printf("%d", kdnum);
andJdmat 0:acd6b856d63c 429 }
andJdmat 0:acd6b856d63c 430 }
andJdmat 0:acd6b856d63c 431 }
andJdmat 0:acd6b856d63c 432 }
andJdmat 0:acd6b856d63c 433 if (!button3) //cambia la posicion de ingreso de parametros
andJdmat 0:acd6b856d63c 434 {
andJdmat 0:acd6b856d63c 435 led3 =!led3;
andJdmat 0:acd6b856d63c 436 if(pos==4)
andJdmat 0:acd6b856d63c 437 {
andJdmat 0:acd6b856d63c 438 pos=1;
andJdmat 0:acd6b856d63c 439 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 440 lcd.printf("%d", spnum);
andJdmat 0:acd6b856d63c 441 }
andJdmat 0:acd6b856d63c 442 else if (pos==1)
andJdmat 0:acd6b856d63c 443 {
andJdmat 0:acd6b856d63c 444 pos++;
andJdmat 0:acd6b856d63c 445 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 446 lcd.printf("%d", kpnum);
andJdmat 0:acd6b856d63c 447 }
andJdmat 0:acd6b856d63c 448 else if(pos==2)
andJdmat 0:acd6b856d63c 449 {
andJdmat 0:acd6b856d63c 450 pos++;
andJdmat 0:acd6b856d63c 451 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 452 lcd.printf("%d", kinum);
andJdmat 0:acd6b856d63c 453 }
andJdmat 0:acd6b856d63c 454 else if(pos==3)
andJdmat 0:acd6b856d63c 455 {
andJdmat 0:acd6b856d63c 456 pos++;
andJdmat 0:acd6b856d63c 457 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 458 lcd.printf("%d", kdnum);
andJdmat 0:acd6b856d63c 459 }
andJdmat 0:acd6b856d63c 460 wait(0.25);
andJdmat 0:acd6b856d63c 461
andJdmat 0:acd6b856d63c 462 }
andJdmat 0:acd6b856d63c 463
andJdmat 0:acd6b856d63c 464 if (!button4)
andJdmat 0:acd6b856d63c 465 {
andJdmat 0:acd6b856d63c 466 break; //sale del bucle si pisan suiche4
andJdmat 0:acd6b856d63c 467 }
andJdmat 0:acd6b856d63c 468 wait(0.1);
andJdmat 0:acd6b856d63c 469 }
andJdmat 0:acd6b856d63c 470
andJdmat 0:acd6b856d63c 471
andJdmat 0:acd6b856d63c 472 //Transicion
andJdmat 0:acd6b856d63c 473 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
andJdmat 0:acd6b856d63c 474 lcd.cls(); //borra la pantalla
andJdmat 0:acd6b856d63c 475 lcd.printf(" GUARDADOS!");
andJdmat 0:acd6b856d63c 476 wait(1);
andJdmat 0:acd6b856d63c 477 lcd.cls();
andJdmat 0:acd6b856d63c 478 lcd.printf(" INICIA EL PID");
andJdmat 0:acd6b856d63c 479 wait(1);
andJdmat 0:acd6b856d63c 480 // se imprimen los parches del control *****************************************
andJdmat 0:acd6b856d63c 481 lcd.cls();
andJdmat 0:acd6b856d63c 482 lcd.printf("Er=%3.0f",err);
andJdmat 0:acd6b856d63c 483 lcd.locate(8,0);
andJdmat 0:acd6b856d63c 484 lcd.printf("Me=%3.0f",med);
andJdmat 0:acd6b856d63c 485 lcd.locate(0,1);
andJdmat 0:acd6b856d63c 486 lcd.printf("Sp=%3.0f",spnum);
andJdmat 0:acd6b856d63c 487 lcd.locate(8,1);
andJdmat 0:acd6b856d63c 488 lcd.printf("Co=%3.0f",pid);
andJdmat 0:acd6b856d63c 489 wait(1);
andJdmat 0:acd6b856d63c 490
andJdmat 0:acd6b856d63c 491 // CICLO PRINCIPAL CONTROLADOR PID
andJdmat 0:acd6b856d63c 492 lop1: med = y.read()*999;
andJdmat 0:acd6b856d63c 493 err = (spnum-med); //se calcula el error
andJdmat 0:acd6b856d63c 494 ap = kpnum*err*0.01f; //se calcula la accion proporcinal
andJdmat 0:acd6b856d63c 495 ai =(kinum*err*0.01f)+ai; //calculo de la integral del error
andJdmat 0:acd6b856d63c 496 ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
andJdmat 0:acd6b856d63c 497 pid = (ap+ai+ad);
andJdmat 0:acd6b856d63c 498 // se verifica que pid sea positivo **************************************
andJdmat 0:acd6b856d63c 499 if(pid<=0)
andJdmat 0:acd6b856d63c 500 {
andJdmat 0:acd6b856d63c 501 pid=0;
andJdmat 0:acd6b856d63c 502 }
andJdmat 0:acd6b856d63c 503
andJdmat 0:acd6b856d63c 504 // se verifica que pid sea menor o igual la valor maximo *****************
andJdmat 0:acd6b856d63c 505 if (pid > 999)
andJdmat 0:acd6b856d63c 506 {
andJdmat 0:acd6b856d63c 507 pid=999;
andJdmat 0:acd6b856d63c 508 }
andJdmat 0:acd6b856d63c 509
andJdmat 0:acd6b856d63c 510
andJdmat 0:acd6b856d63c 511 //se muestran las variables******************************************
andJdmat 0:acd6b856d63c 512 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 513 lcd.printf(" ");
andJdmat 0:acd6b856d63c 514 lcd.locate(3,0);
andJdmat 0:acd6b856d63c 515 lcd.printf("%3.0f",err);
andJdmat 0:acd6b856d63c 516 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 517 lcd.printf(" ");
andJdmat 0:acd6b856d63c 518 lcd.locate(11,0);
andJdmat 0:acd6b856d63c 519 lcd.printf("%3.0f",med);
andJdmat 0:acd6b856d63c 520 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 521 lcd.printf(" ");
andJdmat 0:acd6b856d63c 522 lcd.locate(3,1);
andJdmat 0:acd6b856d63c 523 lcd.printf("%d",spnum);
andJdmat 0:acd6b856d63c 524 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 525 lcd.printf(" ");
andJdmat 0:acd6b856d63c 526 lcd.locate(11,1);
andJdmat 0:acd6b856d63c 527 lcd.printf("%3.0f",pid);
andJdmat 0:acd6b856d63c 528
andJdmat 0:acd6b856d63c 529 // Modificacion para enviar los datos por bluetooth
andJdmat 0:acd6b856d63c 530
andJdmat 0:acd6b856d63c 531
andJdmat 0:acd6b856d63c 532 cleanBuffer(err_s,3);
andJdmat 0:acd6b856d63c 533 cleanBuffer(spnum_s,3);
andJdmat 0:acd6b856d63c 534 cleanBuffer(co_s,3);
andJdmat 0:acd6b856d63c 535 cleanBuffer(med_s,3);
andJdmat 0:acd6b856d63c 536 cleanBuffer(salidas,17);
andJdmat 0:acd6b856d63c 537 //pc.printf("error= %f\n\r ",err);
andJdmat 0:acd6b856d63c 538
andJdmat 0:acd6b856d63c 539 char *r = ".";
andJdmat 0:acd6b856d63c 540
andJdmat 0:acd6b856d63c 541
andJdmat 0:acd6b856d63c 542 itoa(int(err),err_s,10);
andJdmat 0:acd6b856d63c 543 //pc.printf("error= %s\n\r ",err_s);
andJdmat 0:acd6b856d63c 544 strcpy(salidas, err_s);
andJdmat 0:acd6b856d63c 545 strcat(salidas, r);
andJdmat 0:acd6b856d63c 546
andJdmat 0:acd6b856d63c 547 itoa(int(spnum),spnum_s,10);
andJdmat 0:acd6b856d63c 548 //pc.printf("sp= %s\n\r ",spnum_s);
andJdmat 0:acd6b856d63c 549 strcat(salidas, spnum_s);
andJdmat 0:acd6b856d63c 550 strcat(salidas, r);
andJdmat 0:acd6b856d63c 551
andJdmat 0:acd6b856d63c 552 itoa(int(pid),co_s,10);
andJdmat 0:acd6b856d63c 553 //pc.printf("CO= %s\n\r ",co_s);
andJdmat 0:acd6b856d63c 554 strcat(salidas, co_s);
andJdmat 0:acd6b856d63c 555 strcat(salidas, r);
andJdmat 0:acd6b856d63c 556
andJdmat 0:acd6b856d63c 557 itoa(int(med),med_s,10);
andJdmat 0:acd6b856d63c 558 //pc.printf("med= %s\n\r ",med_s);
andJdmat 0:acd6b856d63c 559 strcat(salidas, med_s);
andJdmat 0:acd6b856d63c 560 strcat(salidas, r);
andJdmat 0:acd6b856d63c 561 salidas[17]='.';
andJdmat 0:acd6b856d63c 562 pc.printf("Salidas= %s\n\r ",salidas);
andJdmat 0:acd6b856d63c 563
andJdmat 0:acd6b856d63c 564
andJdmat 0:acd6b856d63c 565 device.puts(salidas);
andJdmat 0:acd6b856d63c 566
andJdmat 0:acd6b856d63c 567 cleanBuffer(err_s,3);
andJdmat 0:acd6b856d63c 568 cleanBuffer(spnum_s,3);
andJdmat 0:acd6b856d63c 569 cleanBuffer(co_s,3);
andJdmat 0:acd6b856d63c 570 cleanBuffer(med_s,3);
andJdmat 0:acd6b856d63c 571 cleanBuffer(salidas,17);
andJdmat 0:acd6b856d63c 572 /*
andJdmat 0:acd6b856d63c 573 if(err<256)
andJdmat 0:acd6b856d63c 574 { //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
andJdmat 0:acd6b856d63c 575 device.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
andJdmat 0:acd6b856d63c 576 device.putc(err); //luego la cifra menos significativa
andJdmat 0:acd6b856d63c 577 }
andJdmat 0:acd6b856d63c 578 if(err>255)
andJdmat 0:acd6b856d63c 579 { //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
andJdmat 0:acd6b856d63c 580 j=err/256; //calculo la cifra mas significativa
andJdmat 0:acd6b856d63c 581 k=err-j*256; //calculo la cifra menos significativa
andJdmat 0:acd6b856d63c 582 device.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
andJdmat 0:acd6b856d63c 583 device.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
andJdmat 0:acd6b856d63c 584 }
andJdmat 0:acd6b856d63c 585
andJdmat 0:acd6b856d63c 586 if(med<256)
andJdmat 0:acd6b856d63c 587
andJdmat 0:acd6b856d63c 588 { //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
andJdmat 0:acd6b856d63c 589 device.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
andJdmat 0:acd6b856d63c 590 device.putc(spnum); //luego la cifra menos significativa
andJdmat 0:acd6b856d63c 591 }
andJdmat 0:acd6b856d63c 592
andJdmat 0:acd6b856d63c 593 if(med>255)
andJdmat 0:acd6b856d63c 594 { //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
andJdmat 0:acd6b856d63c 595 j = 0;
andJdmat 0:acd6b856d63c 596 k = 0;
andJdmat 0:acd6b856d63c 597 j=med/256; //calculo la cifra mas significativa
andJdmat 0:acd6b856d63c 598 k=med-j*256; //calculo la cifra menos significativa
andJdmat 0:acd6b856d63c 599 device.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
andJdmat 0:acd6b856d63c 600 device.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
andJdmat 0:acd6b856d63c 601 }
andJdmat 0:acd6b856d63c 602 */ // Fin de la modificacion
andJdmat 0:acd6b856d63c 603
andJdmat 0:acd6b856d63c 604 //Normalizacion de la salida
andJdmat 0:acd6b856d63c 605 // se actualizan las variables *******************************************
andJdmat 0:acd6b856d63c 606 err_v = err;
andJdmat 0:acd6b856d63c 607 o = pid/999;
andJdmat 0:acd6b856d63c 608 u.write(o);
andJdmat 0:acd6b856d63c 609 // se envia el valor pid a puerto analogico de salida (D/A) **************
andJdmat 0:acd6b856d63c 610
andJdmat 0:acd6b856d63c 611 // se repite el ciclo
andJdmat 0:acd6b856d63c 612 wait_ms(300);
andJdmat 0:acd6b856d63c 613 goto lop1;
andJdmat 0:acd6b856d63c 614 }