deiwid ricaurte / Mbed 2 deprecated pid_android_ecoder

pid control android

Dependencies:   QEI TextLCD mbed

main.cpp@0:5a12b85e8aa4, 2016-10-07 (annotated)

Committer:
deiwidricaurte
Date:
Fri Oct 07 16:28:53 2016 +0000
Revision:
0:5a12b85e8aa4
control pid con encoder y arduino;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
deiwidricaurte 0:5a12b85e8aa4 1 #include "mbed.h"
deiwidricaurte 0:5a12b85e8aa4 2 #include <stdio.h>
deiwidricaurte 0:5a12b85e8aa4 3 #include "QEI.h"
deiwidricaurte 0:5a12b85e8aa4 4 #include "TextLCD.h"
deiwidricaurte 0:5a12b85e8aa4 5 #include "string.h"
deiwidricaurte 0:5a12b85e8aa4 6
deiwidricaurte 0:5a12b85e8aa4 7
deiwidricaurte 0:5a12b85e8aa4 8 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
deiwidricaurte 0:5a12b85e8aa4 9 QEI encoder (PTA13, PTD5, NC, 624);
deiwidricaurte 0:5a12b85e8aa4 10 AnalogIn y(PTB3);//entrada analoga
deiwidricaurte 0:5a12b85e8aa4 11 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
deiwidricaurte 0:5a12b85e8aa4 12
deiwidricaurte 0:5a12b85e8aa4 13 Serial usart(PTE0,PTE1); //puertos del FRDM para el modem
deiwidricaurte 0:5a12b85e8aa4 14 Serial pc(USBTX,USBRX); //puertos del PC
deiwidricaurte 0:5a12b85e8aa4 15
deiwidricaurte 0:5a12b85e8aa4 16 DigitalOut led1(LED1);
deiwidricaurte 0:5a12b85e8aa4 17 DigitalOut led2(LED2);
deiwidricaurte 0:5a12b85e8aa4 18 DigitalOut led3(LED3);
deiwidricaurte 0:5a12b85e8aa4 19
deiwidricaurte 0:5a12b85e8aa4 20 DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros
deiwidricaurte 0:5a12b85e8aa4 21 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
deiwidricaurte 0:5a12b85e8aa4 22
deiwidricaurte 0:5a12b85e8aa4 23 //codigos movimiento del curzor
deiwidricaurte 0:5a12b85e8aa4 24
deiwidricaurte 0:5a12b85e8aa4 25 //int C1=0x0E; // solo muestra el curzor
deiwidricaurte 0:5a12b85e8aa4 26 int C2=0x18; // desplaza izquierda
deiwidricaurte 0:5a12b85e8aa4 27 int C3=0x1A; // desplaza derecha
deiwidricaurte 0:5a12b85e8aa4 28 int C4=0x0C; // quito cursor bajo
deiwidricaurte 0:5a12b85e8aa4 29
deiwidricaurte 0:5a12b85e8aa4 30 int C1=0x0F;
deiwidricaurte 0:5a12b85e8aa4 31 float pid,o,ai,ad,ap,med,err;
deiwidricaurte 0:5a12b85e8aa4 32 float err_v,med_v;
deiwidricaurte 0:5a12b85e8aa4 33 int sp=0,ki=0,kp=0,kd=0;
deiwidricaurte 0:5a12b85e8aa4 34 int j,k;
deiwidricaurte 0:5a12b85e8aa4 35 char buffer[4];// TAMAÑO DEL BUFER
deiwidricaurte 0:5a12b85e8aa4 36 Timer t; //VALOR DEL TIEMPO
deiwidricaurte 0:5a12b85e8aa4 37 int count;
deiwidricaurte 0:5a12b85e8aa4 38 int contador;
deiwidricaurte 0:5a12b85e8aa4 39 int i = 0;
deiwidricaurte 0:5a12b85e8aa4 40 int c=0;
deiwidricaurte 0:5a12b85e8aa4 41 char cc;
deiwidricaurte 0:5a12b85e8aa4 42 char caux[]="0000";
deiwidricaurte 0:5a12b85e8aa4 43 int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos
deiwidricaurte 0:5a12b85e8aa4 44 {
deiwidricaurte 0:5a12b85e8aa4 45 int ij=0;
deiwidricaurte 0:5a12b85e8aa4 46 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA
deiwidricaurte 0:5a12b85e8aa4 47 while(1) {
deiwidricaurte 0:5a12b85e8aa4 48 while (usart.readable()) {
deiwidricaurte 0:5a12b85e8aa4 49 //char
deiwidricaurte 0:5a12b85e8aa4 50 cc = usart.getc();
deiwidricaurte 0:5a12b85e8aa4 51 if (cc == '\r' || cc == '\n') cc = '$';//si se envia fin de linea o de caracxter inserta $
deiwidricaurte 0:5a12b85e8aa4 52 buffer[ij++] = cc;//mete al bufer el caracter leido
deiwidricaurte 0:5a12b85e8aa4 53 if(ij > count)break;//sale del loop si ya detecto terminacion
deiwidricaurte 0:5a12b85e8aa4 54 }
deiwidricaurte 0:5a12b85e8aa4 55 if(ij > count)break;
deiwidricaurte 0:5a12b85e8aa4 56 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
deiwidricaurte 0:5a12b85e8aa4 57 t.stop();
deiwidricaurte 0:5a12b85e8aa4 58 t.reset();
deiwidricaurte 0:5a12b85e8aa4 59 break;
deiwidricaurte 0:5a12b85e8aa4 60 }
deiwidricaurte 0:5a12b85e8aa4 61 }
deiwidricaurte 0:5a12b85e8aa4 62 return 0;
deiwidricaurte 0:5a12b85e8aa4 63 }
deiwidricaurte 0:5a12b85e8aa4 64
deiwidricaurte 0:5a12b85e8aa4 65 void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer
deiwidricaurte 0:5a12b85e8aa4 66 {
deiwidricaurte 0:5a12b85e8aa4 67 for(int i=0; i < count; i++) {
deiwidricaurte 0:5a12b85e8aa4 68 buffer[i] = '\0';
deiwidricaurte 0:5a12b85e8aa4 69 }
deiwidricaurte 0:5a12b85e8aa4 70 }
deiwidricaurte 0:5a12b85e8aa4 71 void encoderdatos()
deiwidricaurte 0:5a12b85e8aa4 72 {
deiwidricaurte 0:5a12b85e8aa4 73
deiwidricaurte 0:5a12b85e8aa4 74 int pos=1;
deiwidricaurte 0:5a12b85e8aa4 75 int cambio=0, diferencia=0;
deiwidricaurte 0:5a12b85e8aa4 76
deiwidricaurte 0:5a12b85e8aa4 77 while(1)
deiwidricaurte 0:5a12b85e8aa4 78 {
deiwidricaurte 0:5a12b85e8aa4 79
deiwidricaurte 0:5a12b85e8aa4 80 diferencia=encoder.getPulses()-cambio;
deiwidricaurte 0:5a12b85e8aa4 81 cambio=encoder.getPulses();
deiwidricaurte 0:5a12b85e8aa4 82 if (diferencia==0)
deiwidricaurte 0:5a12b85e8aa4 83 {
deiwidricaurte 0:5a12b85e8aa4 84 //nada
deiwidricaurte 0:5a12b85e8aa4 85 }
deiwidricaurte 0:5a12b85e8aa4 86 else if(diferencia>0)
deiwidricaurte 0:5a12b85e8aa4 87 {
deiwidricaurte 0:5a12b85e8aa4 88 if(pos==1)
deiwidricaurte 0:5a12b85e8aa4 89 {
deiwidricaurte 0:5a12b85e8aa4 90
deiwidricaurte 0:5a12b85e8aa4 91 if(sp+diferencia>=999)
deiwidricaurte 0:5a12b85e8aa4 92 {
deiwidricaurte 0:5a12b85e8aa4 93 sp=999;
deiwidricaurte 0:5a12b85e8aa4 94 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 95 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 96 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 97 lcd.printf("%d", sp);
deiwidricaurte 0:5a12b85e8aa4 98 }
deiwidricaurte 0:5a12b85e8aa4 99 else
deiwidricaurte 0:5a12b85e8aa4 100 {
deiwidricaurte 0:5a12b85e8aa4 101 sp+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 102 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 103 lcd.printf("%d", sp);
deiwidricaurte 0:5a12b85e8aa4 104
deiwidricaurte 0:5a12b85e8aa4 105
deiwidricaurte 0:5a12b85e8aa4 106 }
deiwidricaurte 0:5a12b85e8aa4 107 }
deiwidricaurte 0:5a12b85e8aa4 108 else if(pos==2)
deiwidricaurte 0:5a12b85e8aa4 109 {
deiwidricaurte 0:5a12b85e8aa4 110 if(kp+diferencia>=999)
deiwidricaurte 0:5a12b85e8aa4 111 {
deiwidricaurte 0:5a12b85e8aa4 112 kp=999;
deiwidricaurte 0:5a12b85e8aa4 113 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 114 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 115 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 116 lcd.printf("%d", kp);
deiwidricaurte 0:5a12b85e8aa4 117 }
deiwidricaurte 0:5a12b85e8aa4 118 else
deiwidricaurte 0:5a12b85e8aa4 119 {
deiwidricaurte 0:5a12b85e8aa4 120 kp+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 121 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 122 lcd.printf("%d", kp);
deiwidricaurte 0:5a12b85e8aa4 123 }
deiwidricaurte 0:5a12b85e8aa4 124 }
deiwidricaurte 0:5a12b85e8aa4 125 else if(pos==3)
deiwidricaurte 0:5a12b85e8aa4 126 {
deiwidricaurte 0:5a12b85e8aa4 127 if(ki+diferencia>=999)
deiwidricaurte 0:5a12b85e8aa4 128 {
deiwidricaurte 0:5a12b85e8aa4 129 ki=999;
deiwidricaurte 0:5a12b85e8aa4 130 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 131 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 132 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 133 lcd.printf("%d", ki);
deiwidricaurte 0:5a12b85e8aa4 134 }
deiwidricaurte 0:5a12b85e8aa4 135 else
deiwidricaurte 0:5a12b85e8aa4 136 {
deiwidricaurte 0:5a12b85e8aa4 137 ki+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 138 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 139 lcd.printf("%d", ki);
deiwidricaurte 0:5a12b85e8aa4 140 }
deiwidricaurte 0:5a12b85e8aa4 141 }
deiwidricaurte 0:5a12b85e8aa4 142 else if(pos==4)
deiwidricaurte 0:5a12b85e8aa4 143 {
deiwidricaurte 0:5a12b85e8aa4 144 if(kd+diferencia>=999)
deiwidricaurte 0:5a12b85e8aa4 145 {
deiwidricaurte 0:5a12b85e8aa4 146 kd=999;
deiwidricaurte 0:5a12b85e8aa4 147 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 148 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 149 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 150 lcd.printf("%d", kd);
deiwidricaurte 0:5a12b85e8aa4 151 }
deiwidricaurte 0:5a12b85e8aa4 152 else
deiwidricaurte 0:5a12b85e8aa4 153 {
deiwidricaurte 0:5a12b85e8aa4 154 kd+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 155 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 156 lcd.printf("%d", kd);
deiwidricaurte 0:5a12b85e8aa4 157 }
deiwidricaurte 0:5a12b85e8aa4 158 }
deiwidricaurte 0:5a12b85e8aa4 159 }
deiwidricaurte 0:5a12b85e8aa4 160 else if(diferencia<0)
deiwidricaurte 0:5a12b85e8aa4 161 {
deiwidricaurte 0:5a12b85e8aa4 162 if(pos==1)
deiwidricaurte 0:5a12b85e8aa4 163 {
deiwidricaurte 0:5a12b85e8aa4 164 if(sp+diferencia<0)
deiwidricaurte 0:5a12b85e8aa4 165 {
deiwidricaurte 0:5a12b85e8aa4 166 //No ocurre nada
deiwidricaurte 0:5a12b85e8aa4 167 }
deiwidricaurte 0:5a12b85e8aa4 168 else
deiwidricaurte 0:5a12b85e8aa4 169 {
deiwidricaurte 0:5a12b85e8aa4 170 sp+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 171 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 172 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 173 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 174 lcd.printf("%d", sp);
deiwidricaurte 0:5a12b85e8aa4 175 }
deiwidricaurte 0:5a12b85e8aa4 176 }
deiwidricaurte 0:5a12b85e8aa4 177 else if(pos==2)
deiwidricaurte 0:5a12b85e8aa4 178 {
deiwidricaurte 0:5a12b85e8aa4 179 if(kp+diferencia<0)
deiwidricaurte 0:5a12b85e8aa4 180 {
deiwidricaurte 0:5a12b85e8aa4 181 //No ocurre nada
deiwidricaurte 0:5a12b85e8aa4 182 }
deiwidricaurte 0:5a12b85e8aa4 183 else
deiwidricaurte 0:5a12b85e8aa4 184 {
deiwidricaurte 0:5a12b85e8aa4 185 kp+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 186 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 187 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 188 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 189 lcd.printf("%d", kp);
deiwidricaurte 0:5a12b85e8aa4 190 }
deiwidricaurte 0:5a12b85e8aa4 191 }
deiwidricaurte 0:5a12b85e8aa4 192 else if(pos==3)
deiwidricaurte 0:5a12b85e8aa4 193 {
deiwidricaurte 0:5a12b85e8aa4 194 if(ki+diferencia<0)
deiwidricaurte 0:5a12b85e8aa4 195 {
deiwidricaurte 0:5a12b85e8aa4 196 //No ocurre nada
deiwidricaurte 0:5a12b85e8aa4 197 }
deiwidricaurte 0:5a12b85e8aa4 198 else
deiwidricaurte 0:5a12b85e8aa4 199 {
deiwidricaurte 0:5a12b85e8aa4 200 ki+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 201 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 202 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 203 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 204 lcd.printf("%d", ki);
deiwidricaurte 0:5a12b85e8aa4 205 }
deiwidricaurte 0:5a12b85e8aa4 206 }
deiwidricaurte 0:5a12b85e8aa4 207 else if(pos==4)
deiwidricaurte 0:5a12b85e8aa4 208 {
deiwidricaurte 0:5a12b85e8aa4 209 if(kd+diferencia<0)
deiwidricaurte 0:5a12b85e8aa4 210 {
deiwidricaurte 0:5a12b85e8aa4 211 //No ocurre nada
deiwidricaurte 0:5a12b85e8aa4 212 }
deiwidricaurte 0:5a12b85e8aa4 213 else
deiwidricaurte 0:5a12b85e8aa4 214 {
deiwidricaurte 0:5a12b85e8aa4 215 kd+=diferencia;
deiwidricaurte 0:5a12b85e8aa4 216 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 217 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 218 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 219 lcd.printf("%d", kd);
deiwidricaurte 0:5a12b85e8aa4 220 }
deiwidricaurte 0:5a12b85e8aa4 221 }
deiwidricaurte 0:5a12b85e8aa4 222 }
deiwidricaurte 0:5a12b85e8aa4 223 if (!button3) //cambia la posicion de ingreso de parametros
deiwidricaurte 0:5a12b85e8aa4 224 {
deiwidricaurte 0:5a12b85e8aa4 225 led3 =!led3;
deiwidricaurte 0:5a12b85e8aa4 226 if(pos==4)
deiwidricaurte 0:5a12b85e8aa4 227 {
deiwidricaurte 0:5a12b85e8aa4 228 pos=1;
deiwidricaurte 0:5a12b85e8aa4 229 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 230 lcd.printf("%d", sp);
deiwidricaurte 0:5a12b85e8aa4 231 }
deiwidricaurte 0:5a12b85e8aa4 232 else if (pos==1)
deiwidricaurte 0:5a12b85e8aa4 233 {
deiwidricaurte 0:5a12b85e8aa4 234 pos++;
deiwidricaurte 0:5a12b85e8aa4 235 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 236 lcd.printf("%d", kp);
deiwidricaurte 0:5a12b85e8aa4 237 }
deiwidricaurte 0:5a12b85e8aa4 238 else if(pos==2)
deiwidricaurte 0:5a12b85e8aa4 239 {
deiwidricaurte 0:5a12b85e8aa4 240 pos++;
deiwidricaurte 0:5a12b85e8aa4 241 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 242 lcd.printf("%d", ki);
deiwidricaurte 0:5a12b85e8aa4 243 }
deiwidricaurte 0:5a12b85e8aa4 244 else if(pos==3)
deiwidricaurte 0:5a12b85e8aa4 245 {
deiwidricaurte 0:5a12b85e8aa4 246 pos++;
deiwidricaurte 0:5a12b85e8aa4 247 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 248 lcd.printf("%d", kd);
deiwidricaurte 0:5a12b85e8aa4 249 }
deiwidricaurte 0:5a12b85e8aa4 250 wait(0.25);
deiwidricaurte 0:5a12b85e8aa4 251
deiwidricaurte 0:5a12b85e8aa4 252 }
deiwidricaurte 0:5a12b85e8aa4 253
deiwidricaurte 0:5a12b85e8aa4 254
deiwidricaurte 0:5a12b85e8aa4 255 if (!button4)
deiwidricaurte 0:5a12b85e8aa4 256 {
deiwidricaurte 0:5a12b85e8aa4 257 break; //sale del bucle si pisan suiche4
deiwidricaurte 0:5a12b85e8aa4 258 }
deiwidricaurte 0:5a12b85e8aa4 259
deiwidricaurte 0:5a12b85e8aa4 260 wait(0.01);
deiwidricaurte 0:5a12b85e8aa4 261 }
deiwidricaurte 0:5a12b85e8aa4 262 //return spnum,kinum,kpnum,kdnum;
deiwidricaurte 0:5a12b85e8aa4 263
deiwidricaurte 0:5a12b85e8aa4 264 }
deiwidricaurte 0:5a12b85e8aa4 265
deiwidricaurte 0:5a12b85e8aa4 266 int main()
deiwidricaurte 0:5a12b85e8aa4 267 {
deiwidricaurte 0:5a12b85e8aa4 268
deiwidricaurte 0:5a12b85e8aa4 269
deiwidricaurte 0:5a12b85e8aa4 270 lcd.locate(0,1);
deiwidricaurte 0:5a12b85e8aa4 271 lcd.printf("**Control PID**");
deiwidricaurte 0:5a12b85e8aa4 272 wait(2);
deiwidricaurte 0:5a12b85e8aa4 273 lcd.cls(); // Borrar Pantalla
deiwidricaurte 0:5a12b85e8aa4 274 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
deiwidricaurte 0:5a12b85e8aa4 275
deiwidricaurte 0:5a12b85e8aa4 276 lcd.locate(8,0);
deiwidricaurte 0:5a12b85e8aa4 277 lcd.printf("Kp=%d",kp);
deiwidricaurte 0:5a12b85e8aa4 278 lcd.locate(0,1);
deiwidricaurte 0:5a12b85e8aa4 279 lcd.printf("Ki=%d",ki);
deiwidricaurte 0:5a12b85e8aa4 280 lcd.locate(8,1);
deiwidricaurte 0:5a12b85e8aa4 281 lcd.printf("Kd=%d",kd);
deiwidricaurte 0:5a12b85e8aa4 282 lcd.locate(0,0);
deiwidricaurte 0:5a12b85e8aa4 283 lcd.printf("Sp=%d",sp);
deiwidricaurte 0:5a12b85e8aa4 284 wait(0.2);
deiwidricaurte 0:5a12b85e8aa4 285 encoderdatos();
deiwidricaurte 0:5a12b85e8aa4 286 wait(0.1);
deiwidricaurte 0:5a12b85e8aa4 287
deiwidricaurte 0:5a12b85e8aa4 288 usart.baud(9600); // asigno baudios y configuro puerto serie de la usart
deiwidricaurte 0:5a12b85e8aa4 289 usart.format(8,Serial::None,1);
deiwidricaurte 0:5a12b85e8aa4 290 //
deiwidricaurte 0:5a12b85e8aa4 291
deiwidricaurte 0:5a12b85e8aa4 292
deiwidricaurte 0:5a12b85e8aa4 293 //
deiwidricaurte 0:5a12b85e8aa4 294 //sp=valores[0];
deiwidricaurte 0:5a12b85e8aa4 295
deiwidricaurte 0:5a12b85e8aa4 296 wait(0.001);
deiwidricaurte 0:5a12b85e8aa4 297 if(sp<256)
deiwidricaurte 0:5a12b85e8aa4 298 {
deiwidricaurte 0:5a12b85e8aa4 299 usart.putc(sp);
deiwidricaurte 0:5a12b85e8aa4 300 usart.putc(0);
deiwidricaurte 0:5a12b85e8aa4 301
deiwidricaurte 0:5a12b85e8aa4 302 }
deiwidricaurte 0:5a12b85e8aa4 303 if(sp>255)
deiwidricaurte 0:5a12b85e8aa4 304 {
deiwidricaurte 0:5a12b85e8aa4 305 j=sp/256;
deiwidricaurte 0:5a12b85e8aa4 306 k=sp-j*256;
deiwidricaurte 0:5a12b85e8aa4 307 usart.putc(k);
deiwidricaurte 0:5a12b85e8aa4 308 usart.putc(j);
deiwidricaurte 0:5a12b85e8aa4 309
deiwidricaurte 0:5a12b85e8aa4 310 }
deiwidricaurte 0:5a12b85e8aa4 311 wait(0.001);
deiwidricaurte 0:5a12b85e8aa4 312 if(kp<256)
deiwidricaurte 0:5a12b85e8aa4 313 {
deiwidricaurte 0:5a12b85e8aa4 314 usart.putc(kp);
deiwidricaurte 0:5a12b85e8aa4 315 usart.putc(0);
deiwidricaurte 0:5a12b85e8aa4 316
deiwidricaurte 0:5a12b85e8aa4 317 }
deiwidricaurte 0:5a12b85e8aa4 318 if(kp>255)
deiwidricaurte 0:5a12b85e8aa4 319 {
deiwidricaurte 0:5a12b85e8aa4 320 j=kp/256;
deiwidricaurte 0:5a12b85e8aa4 321 k=kp-j*256;
deiwidricaurte 0:5a12b85e8aa4 322 usart.putc(k);
deiwidricaurte 0:5a12b85e8aa4 323 usart.putc(j);
deiwidricaurte 0:5a12b85e8aa4 324
deiwidricaurte 0:5a12b85e8aa4 325 }
deiwidricaurte 0:5a12b85e8aa4 326 wait(0.001);
deiwidricaurte 0:5a12b85e8aa4 327 if(ki<256)
deiwidricaurte 0:5a12b85e8aa4 328 {
deiwidricaurte 0:5a12b85e8aa4 329
deiwidricaurte 0:5a12b85e8aa4 330 usart.putc(ki);
deiwidricaurte 0:5a12b85e8aa4 331 usart.putc(0);
deiwidricaurte 0:5a12b85e8aa4 332
deiwidricaurte 0:5a12b85e8aa4 333 }
deiwidricaurte 0:5a12b85e8aa4 334 if(ki>255)
deiwidricaurte 0:5a12b85e8aa4 335 {
deiwidricaurte 0:5a12b85e8aa4 336 j=ki/256;
deiwidricaurte 0:5a12b85e8aa4 337 k=ki-j*256;
deiwidricaurte 0:5a12b85e8aa4 338 usart.putc(k);
deiwidricaurte 0:5a12b85e8aa4 339 usart.putc(j);
deiwidricaurte 0:5a12b85e8aa4 340
deiwidricaurte 0:5a12b85e8aa4 341 }
deiwidricaurte 0:5a12b85e8aa4 342 wait(0.001);
deiwidricaurte 0:5a12b85e8aa4 343 if(kd<256)
deiwidricaurte 0:5a12b85e8aa4 344 {
deiwidricaurte 0:5a12b85e8aa4 345 usart.putc(kd);
deiwidricaurte 0:5a12b85e8aa4 346 usart.putc(0);
deiwidricaurte 0:5a12b85e8aa4 347
deiwidricaurte 0:5a12b85e8aa4 348 }
deiwidricaurte 0:5a12b85e8aa4 349 if(sp>255)
deiwidricaurte 0:5a12b85e8aa4 350 {
deiwidricaurte 0:5a12b85e8aa4 351 j=kd/256;
deiwidricaurte 0:5a12b85e8aa4 352 k=kd-j*256;
deiwidricaurte 0:5a12b85e8aa4 353 usart.putc(k);
deiwidricaurte 0:5a12b85e8aa4 354 usart.putc(j);
deiwidricaurte 0:5a12b85e8aa4 355
deiwidricaurte 0:5a12b85e8aa4 356 }
deiwidricaurte 0:5a12b85e8aa4 357
deiwidricaurte 0:5a12b85e8aa4 358 //Transicion
deiwidricaurte 0:5a12b85e8aa4 359 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
deiwidricaurte 0:5a12b85e8aa4 360 lcd.cls(); //borra la pantalla
deiwidricaurte 0:5a12b85e8aa4 361 lcd.printf(" GUARDADOS!");
deiwidricaurte 0:5a12b85e8aa4 362 wait(1);
deiwidricaurte 0:5a12b85e8aa4 363 lcd.cls();
deiwidricaurte 0:5a12b85e8aa4 364 lcd.printf(" INICIA EL PID");
deiwidricaurte 0:5a12b85e8aa4 365 wait(1);
deiwidricaurte 0:5a12b85e8aa4 366 // se imprimen los parches del control *****************************************
deiwidricaurte 0:5a12b85e8aa4 367 lcd.cls();
deiwidricaurte 0:5a12b85e8aa4 368 lcd.printf("Er=%3.0f",err);
deiwidricaurte 0:5a12b85e8aa4 369 lcd.locate(8,0);
deiwidricaurte 0:5a12b85e8aa4 370 lcd.printf("Me=%3.0f",med);
deiwidricaurte 0:5a12b85e8aa4 371 lcd.locate(0,1);
deiwidricaurte 0:5a12b85e8aa4 372 lcd.printf("Sp=%3.0f",sp);
deiwidricaurte 0:5a12b85e8aa4 373 lcd.locate(8,1);
deiwidricaurte 0:5a12b85e8aa4 374 lcd.printf("Co=%3.0f",pid);
deiwidricaurte 0:5a12b85e8aa4 375 wait(1);
deiwidricaurte 0:5a12b85e8aa4 376
deiwidricaurte 0:5a12b85e8aa4 377 //ciclo control
deiwidricaurte 0:5a12b85e8aa4 378
deiwidricaurte 0:5a12b85e8aa4 379 while(1)
deiwidricaurte 0:5a12b85e8aa4 380 {
deiwidricaurte 0:5a12b85e8aa4 381 //
deiwidricaurte 0:5a12b85e8aa4 382 if (usart.readable())
deiwidricaurte 0:5a12b85e8aa4 383 {
deiwidricaurte 0:5a12b85e8aa4 384 cleanBuffer(buffer,4);
deiwidricaurte 0:5a12b85e8aa4 385 readBuffer(buffer,4);
deiwidricaurte 0:5a12b85e8aa4 386 pc.printf("buffer= %s\n\r ",buffer); //imprime el bufer
deiwidricaurte 0:5a12b85e8aa4 387
deiwidricaurte 0:5a12b85e8aa4 388 if ('h' == buffer[0])
deiwidricaurte 0:5a12b85e8aa4 389 {
deiwidricaurte 0:5a12b85e8aa4 390 lcd.cls();
deiwidricaurte 0:5a12b85e8aa4 391 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 392 lcd.printf("hola: %s",buffer);
deiwidricaurte 0:5a12b85e8aa4 393 wait(1);
deiwidricaurte 0:5a12b85e8aa4 394 lcd.cls();
deiwidricaurte 0:5a12b85e8aa4 395 //cleanBuffer(buffer,4);
deiwidricaurte 0:5a12b85e8aa4 396 //sp=88;
deiwidricaurte 0:5a12b85e8aa4 397 for(int ii=1; ii < 4; ii++){
deiwidricaurte 0:5a12b85e8aa4 398 caux[ii-1]=buffer[ii];
deiwidricaurte 0:5a12b85e8aa4 399
deiwidricaurte 0:5a12b85e8aa4 400 }
deiwidricaurte 0:5a12b85e8aa4 401 //sp=caux[2];
deiwidricaurte 0:5a12b85e8aa4 402
deiwidricaurte 0:5a12b85e8aa4 403 }
deiwidricaurte 0:5a12b85e8aa4 404 //sp=strtol(caux,null,2);
deiwidricaurte 0:5a12b85e8aa4 405
deiwidricaurte 0:5a12b85e8aa4 406 }
deiwidricaurte 0:5a12b85e8aa4 407
deiwidricaurte 0:5a12b85e8aa4 408
deiwidricaurte 0:5a12b85e8aa4 409 //
deiwidricaurte 0:5a12b85e8aa4 410 med = y.read()*999;
deiwidricaurte 0:5a12b85e8aa4 411 err = (sp-med); //se calcula el error
deiwidricaurte 0:5a12b85e8aa4 412 ap = kp*err*0.01f; //se calcula la accion proporcinal
deiwidricaurte 0:5a12b85e8aa4 413 ai =(ki*err*0.01f)+ai; //calculo de la integral del error
deiwidricaurte 0:5a12b85e8aa4 414 ad = kd*(err-err_v)*0.01f; //calculo de la accion derivativa
deiwidricaurte 0:5a12b85e8aa4 415 pid = (ap+ai+ad);
deiwidricaurte 0:5a12b85e8aa4 416 // se verifica que pid sea positivo **************************************
deiwidricaurte 0:5a12b85e8aa4 417 if(pid<=0)
deiwidricaurte 0:5a12b85e8aa4 418 {
deiwidricaurte 0:5a12b85e8aa4 419 pid=0;
deiwidricaurte 0:5a12b85e8aa4 420 }
deiwidricaurte 0:5a12b85e8aa4 421
deiwidricaurte 0:5a12b85e8aa4 422 // se verifica que pid sea menor o igual la valor maximo *****************
deiwidricaurte 0:5a12b85e8aa4 423 if (pid > 999)
deiwidricaurte 0:5a12b85e8aa4 424 {
deiwidricaurte 0:5a12b85e8aa4 425 pid=999;
deiwidricaurte 0:5a12b85e8aa4 426 }
deiwidricaurte 0:5a12b85e8aa4 427
deiwidricaurte 0:5a12b85e8aa4 428
deiwidricaurte 0:5a12b85e8aa4 429 //se muestran las variables******************************************
deiwidricaurte 0:5a12b85e8aa4 430 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 431 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 432 lcd.locate(3,0);
deiwidricaurte 0:5a12b85e8aa4 433 lcd.printf("%3.0f",err);
deiwidricaurte 0:5a12b85e8aa4 434 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 435 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 436 lcd.locate(11,0);
deiwidricaurte 0:5a12b85e8aa4 437 lcd.printf("%3.0f",med);
deiwidricaurte 0:5a12b85e8aa4 438 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 439 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 440 lcd.locate(3,1);
deiwidricaurte 0:5a12b85e8aa4 441 lcd.printf("%d",sp);
deiwidricaurte 0:5a12b85e8aa4 442 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 443 lcd.printf(" ");
deiwidricaurte 0:5a12b85e8aa4 444 lcd.locate(11,1);
deiwidricaurte 0:5a12b85e8aa4 445 lcd.printf("%3.0f",pid);
deiwidricaurte 0:5a12b85e8aa4 446
deiwidricaurte 0:5a12b85e8aa4 447
deiwidricaurte 0:5a12b85e8aa4 448
deiwidricaurte 0:5a12b85e8aa4 449
deiwidricaurte 0:5a12b85e8aa4 450 //Normalizacion de la salida
deiwidricaurte 0:5a12b85e8aa4 451 // se actualizan las variables *******************************************
deiwidricaurte 0:5a12b85e8aa4 452 err_v = err;
deiwidricaurte 0:5a12b85e8aa4 453 if(err_v<0)
deiwidricaurte 0:5a12b85e8aa4 454 {
deiwidricaurte 0:5a12b85e8aa4 455 err_v=err_v*(-1);
deiwidricaurte 0:5a12b85e8aa4 456 }
deiwidricaurte 0:5a12b85e8aa4 457
deiwidricaurte 0:5a12b85e8aa4 458 if(err_v<256)
deiwidricaurte 0:5a12b85e8aa4 459 {
deiwidricaurte 0:5a12b85e8aa4 460 usart.putc(err_v);
deiwidricaurte 0:5a12b85e8aa4 461 usart.putc(0);
deiwidricaurte 0:5a12b85e8aa4 462
deiwidricaurte 0:5a12b85e8aa4 463 }
deiwidricaurte 0:5a12b85e8aa4 464
deiwidricaurte 0:5a12b85e8aa4 465 if(err_v>255)
deiwidricaurte 0:5a12b85e8aa4 466 {
deiwidricaurte 0:5a12b85e8aa4 467 j=err_v/256;
deiwidricaurte 0:5a12b85e8aa4 468 k=err_v-j*256;
deiwidricaurte 0:5a12b85e8aa4 469 usart.putc(k);
deiwidricaurte 0:5a12b85e8aa4 470 usart.putc(j);
deiwidricaurte 0:5a12b85e8aa4 471
deiwidricaurte 0:5a12b85e8aa4 472 }
deiwidricaurte 0:5a12b85e8aa4 473 //wait(0.1);
deiwidricaurte 0:5a12b85e8aa4 474
deiwidricaurte 0:5a12b85e8aa4 475 med_v = med;
deiwidricaurte 0:5a12b85e8aa4 476 //wait(0.1);
deiwidricaurte 0:5a12b85e8aa4 477 if(med_v<256)
deiwidricaurte 0:5a12b85e8aa4 478 {
deiwidricaurte 0:5a12b85e8aa4 479 usart.putc(med_v);
deiwidricaurte 0:5a12b85e8aa4 480 usart.putc(0);
deiwidricaurte 0:5a12b85e8aa4 481
deiwidricaurte 0:5a12b85e8aa4 482 }
deiwidricaurte 0:5a12b85e8aa4 483
deiwidricaurte 0:5a12b85e8aa4 484 if(med_v>255)
deiwidricaurte 0:5a12b85e8aa4 485 {
deiwidricaurte 0:5a12b85e8aa4 486 j=med_v/256;
deiwidricaurte 0:5a12b85e8aa4 487 k=med_v-j*256;
deiwidricaurte 0:5a12b85e8aa4 488 usart.putc(k);
deiwidricaurte 0:5a12b85e8aa4 489 usart.putc(j);
deiwidricaurte 0:5a12b85e8aa4 490
deiwidricaurte 0:5a12b85e8aa4 491 }
deiwidricaurte 0:5a12b85e8aa4 492 //GSM.putc(err_v);
deiwidricaurte 0:5a12b85e8aa4 493 o = pid/999;
deiwidricaurte 0:5a12b85e8aa4 494 u.write(o);
deiwidricaurte 0:5a12b85e8aa4 495 // se envia el valor pid a puerto analogico de salida (D/A) **************
deiwidricaurte 0:5a12b85e8aa4 496
deiwidricaurte 0:5a12b85e8aa4 497 // se repite el ciclo
deiwidricaurte 0:5a12b85e8aa4 498 wait_ms(300);
deiwidricaurte 0:5a12b85e8aa4 499 }
deiwidricaurte 0:5a12b85e8aa4 500
deiwidricaurte 0:5a12b85e8aa4 501
deiwidricaurte 0:5a12b85e8aa4 502 }