Nicolás Villegas Echavarría / Mbed 2 deprecated PID

a

Dependencies:   QEI TextLCD mbed

Fork of PID_ENCODER_OK by Gustavo Ramirez

main.cpp@2:00db719ebd81, 2017-05-10 (annotated)

Committer:
NicolasV
Date:
Wed May 10 22:36:03 2017 +0000
Revision:
2:00db719ebd81
Parent:
1:058b8f5c135d 
v1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NicolasV 2:00db719ebd81 1 /*
NicolasV 2:00db719ebd81 2 * Código por: Laura Álvila
NicolasV 2:00db719ebd81 3 * Daniela López
NicolasV 2:00db719ebd81 4 * Nicolás Villegas
NicolasV 2:00db719ebd81 5 */
NicolasV 2:00db719ebd81 6
tony63 0:4e0dfcf0e7ce 7 #include "mbed.h"
tony63 0:4e0dfcf0e7ce 8 #include "QEI.h"
tony63 0:4e0dfcf0e7ce 9 #include "TextLCD.h"
tony63 0:4e0dfcf0e7ce 10
NicolasV 2:00db719ebd81 11 Serial GSM(PTE0,PTE1); // Módulo Bluetooh
tony63 0:4e0dfcf0e7ce 12 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
NicolasV 2:00db719ebd81 13 QEI encoder (PTA13, PTD5, NC, 1000);
NicolasV 2:00db719ebd81 14
NicolasV 2:00db719ebd81 15
NicolasV 2:00db719ebd81 16 AnalogIn y(PTC2); // Entrada análoga: Salida del sistema
NicolasV 2:00db719ebd81 17 AnalogOut u(PTE30); // Salida análoga: Señal de control
NicolasV 2:00db719ebd81 18
tony63 0:4e0dfcf0e7ce 19 DigitalOut led1(LED1);
tony63 0:4e0dfcf0e7ce 20 DigitalOut led2(LED2);
tony63 0:4e0dfcf0e7ce 21 DigitalOut led3(LED3);
tony63 0:4e0dfcf0e7ce 22
NicolasV 2:00db719ebd81 23 DigitalIn button3(PTC16); // Botón del encoder
NicolasV 2:00db719ebd81 24 DigitalIn button4(PTC17); // Pulsador
tony63 0:4e0dfcf0e7ce 25
NicolasV 2:00db719ebd81 26 // Códigos movimiento del cursor:
NicolasV 2:00db719ebd81 27 int C1=0x0F;
tony63 0:4e0dfcf0e7ce 28 int C2=0x18; // desplaza izquierda
tony63 0:4e0dfcf0e7ce 29 int C3=0x1A; // desplaza derecha
tony63 0:4e0dfcf0e7ce 30 int C4=0x0C; // quito cursor bajo
tony63 0:4e0dfcf0e7ce 31
NicolasV 2:00db719ebd81 32 int cambio = 0, diferencia = 0;
NicolasV 2:00db719ebd81 33 float pid, o, ai, ad, ap, med, err;
tony63 1:058b8f5c135d 34 float err_v;
NicolasV 2:00db719ebd81 35 int spnum = 0, kinum = 0, kpnum = 0 ,kdnum = 0, pos = 1;
NicolasV 2:00db719ebd81 36 int j,k;
tony63 0:4e0dfcf0e7ce 37
NicolasV 2:00db719ebd81 38 int main() {
NicolasV 2:00db719ebd81 39 // Se asigna baudrate y se configura el puerto serie de la USART
NicolasV 2:00db719ebd81 40 GSM.baud(9600);
NicolasV 2:00db719ebd81 41 GSM.format(8,Serial::None,1);
NicolasV 2:00db719ebd81 42
tony63 0:4e0dfcf0e7ce 43 lcd.locate(0,1);
tony63 0:4e0dfcf0e7ce 44 lcd.printf("**Control PID**");
NicolasV 2:00db719ebd81 45 wait(3);
NicolasV 2:00db719ebd81 46
NicolasV 2:00db719ebd81 47 lcd.cls(); // Borrar Pantalla
NicolasV 2:00db719ebd81 48 lcd.writeCommand(C1); // Escribimos un comando segun el manual del modulo LCD
tony63 0:4e0dfcf0e7ce 49
tony63 0:4e0dfcf0e7ce 50 lcd.locate(8,0);
NicolasV 2:00db719ebd81 51 lcd.printf("Kp=%d", kpnum);
tony63 0:4e0dfcf0e7ce 52 lcd.locate(0,1);
NicolasV 2:00db719ebd81 53 lcd.printf("Ki=%d", kinum);
tony63 0:4e0dfcf0e7ce 54 lcd.locate(8,1);
NicolasV 2:00db719ebd81 55 lcd.printf("Kd=%d", kdnum);
tony63 0:4e0dfcf0e7ce 56 lcd.locate(0,0);
NicolasV 2:00db719ebd81 57 lcd.printf("Sp=%d", spnum);
tony63 0:4e0dfcf0e7ce 58
NicolasV 2:00db719ebd81 59 while(true) {
tony63 0:4e0dfcf0e7ce 60
NicolasV 2:00db719ebd81 61 diferencia=encoder.getPulses() - cambio;
tony63 0:4e0dfcf0e7ce 62 cambio=encoder.getPulses();
NicolasV 2:00db719ebd81 63
NicolasV 2:00db719ebd81 64 if (diferencia != 0) {
NicolasV 2:00db719ebd81 65 switch (pos) {
NicolasV 2:00db719ebd81 66 case 1:
NicolasV 2:00db719ebd81 67 spnum += diferencia;
NicolasV 2:00db719ebd81 68
NicolasV 2:00db719ebd81 69 if (spnum >= 999)
NicolasV 2:00db719ebd81 70 spnum = 999;
NicolasV 2:00db719ebd81 71 else if (spnum < 0)
NicolasV 2:00db719ebd81 72 spnum = 0;
NicolasV 2:00db719ebd81 73
tony63 0:4e0dfcf0e7ce 74 lcd.locate(3,0);
tony63 0:4e0dfcf0e7ce 75 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 76 lcd.locate(3,0);
tony63 0:4e0dfcf0e7ce 77 lcd.printf("%d", spnum);
NicolasV 2:00db719ebd81 78 break;
NicolasV 2:00db719ebd81 79 case 2:
NicolasV 2:00db719ebd81 80 kpnum += diferencia;
NicolasV 2:00db719ebd81 81
NicolasV 2:00db719ebd81 82 if (kpnum >= 999)
NicolasV 2:00db719ebd81 83 kpnum = 999;
NicolasV 2:00db719ebd81 84 else if (kpnum < 0)
NicolasV 2:00db719ebd81 85 kpnum = 0;
tony63 0:4e0dfcf0e7ce 86 lcd.locate(11,0);
tony63 0:4e0dfcf0e7ce 87 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 88 lcd.locate(11,0);
tony63 0:4e0dfcf0e7ce 89 lcd.printf("%d", kpnum);
NicolasV 2:00db719ebd81 90 break;
NicolasV 2:00db719ebd81 91 case 3:
NicolasV 2:00db719ebd81 92 kinum += diferencia;
NicolasV 2:00db719ebd81 93
NicolasV 2:00db719ebd81 94 if (kinum >= 999)
NicolasV 2:00db719ebd81 95 kinum = 999;
NicolasV 2:00db719ebd81 96 else if (kinum < 0)
NicolasV 2:00db719ebd81 97 kinum = 0;
NicolasV 2:00db719ebd81 98
tony63 0:4e0dfcf0e7ce 99 lcd.locate(3,1);
tony63 0:4e0dfcf0e7ce 100 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 101 lcd.locate(3,1);
tony63 0:4e0dfcf0e7ce 102 lcd.printf("%d", kinum);
NicolasV 2:00db719ebd81 103 break;
NicolasV 2:00db719ebd81 104 case 4:
NicolasV 2:00db719ebd81 105 kdnum += diferencia;
NicolasV 2:00db719ebd81 106
NicolasV 2:00db719ebd81 107 if (kdnum >= 999)
NicolasV 2:00db719ebd81 108 kdnum = 999;
NicolasV 2:00db719ebd81 109 else if (kdnum < 0)
NicolasV 2:00db719ebd81 110 kdnum = 0;
NicolasV 2:00db719ebd81 111
tony63 0:4e0dfcf0e7ce 112 lcd.locate(11,1);
tony63 0:4e0dfcf0e7ce 113 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 114 lcd.locate(11,1);
tony63 0:4e0dfcf0e7ce 115 lcd.printf("%d", kdnum);
NicolasV 2:00db719ebd81 116 break;
NicolasV 2:00db719ebd81 117 default:
NicolasV 2:00db719ebd81 118 break;
NicolasV 2:00db719ebd81 119 } // Fin switch
NicolasV 2:00db719ebd81 120 } // Fin if
NicolasV 2:00db719ebd81 121
NicolasV 2:00db719ebd81 122 if (!button3) { // Cambia la posición de ingreso de parámetros
tony63 0:4e0dfcf0e7ce 123
NicolasV 2:00db719ebd81 124 led3 = !led3;
NicolasV 2:00db719ebd81 125
NicolasV 2:00db719ebd81 126 switch(pos++) {
NicolasV 2:00db719ebd81 127 case 4:
NicolasV 2:00db719ebd81 128 pos = 1;
tony63 0:4e0dfcf0e7ce 129 lcd.locate(3,0);
tony63 0:4e0dfcf0e7ce 130 lcd.printf("%d", spnum);
NicolasV 2:00db719ebd81 131 break;
NicolasV 2:00db719ebd81 132 case 1:
tony63 0:4e0dfcf0e7ce 133 lcd.locate(11,0);
tony63 0:4e0dfcf0e7ce 134 lcd.printf("%d", kpnum);
NicolasV 2:00db719ebd81 135 break;
NicolasV 2:00db719ebd81 136 case 2:
tony63 0:4e0dfcf0e7ce 137 lcd.locate(3,1);
tony63 0:4e0dfcf0e7ce 138 lcd.printf("%d", kinum);
NicolasV 2:00db719ebd81 139 break;
NicolasV 2:00db719ebd81 140 case 3:
tony63 0:4e0dfcf0e7ce 141 lcd.locate(11,1);
tony63 0:4e0dfcf0e7ce 142 lcd.printf("%d", kdnum);
NicolasV 2:00db719ebd81 143 break;
NicolasV 2:00db719ebd81 144 default:
NicolasV 2:00db719ebd81 145 break;
NicolasV 2:00db719ebd81 146 } // Fin switch
NicolasV 2:00db719ebd81 147 wait(0.25);
NicolasV 2:00db719ebd81 148 } // Fin if
NicolasV 2:00db719ebd81 149
tony63 0:4e0dfcf0e7ce 150
NicolasV 2:00db719ebd81 151 if (!button4) {
tony63 0:4e0dfcf0e7ce 152 break; //sale del bucle si pisan suiche4
tony63 0:4e0dfcf0e7ce 153 }
tony63 0:4e0dfcf0e7ce 154 wait(0.1);
tony63 0:4e0dfcf0e7ce 155 }
tony63 0:4e0dfcf0e7ce 156
tony63 0:4e0dfcf0e7ce 157
NicolasV 2:00db719ebd81 158 // Transición
NicolasV 2:00db719ebd81 159 lcd.writeCommand(C4); // Escribimos un comando segun el manual del módulo LCD para quitar cursor bajo
NicolasV 2:00db719ebd81 160 lcd.cls(); // Borra la pantalla
tony63 0:4e0dfcf0e7ce 161 lcd.printf(" GUARDADOS!");
tony63 0:4e0dfcf0e7ce 162 wait(1);
tony63 0:4e0dfcf0e7ce 163 lcd.cls();
NicolasV 2:00db719ebd81 164 lcd.printf(" *** PID ***");
tony63 0:4e0dfcf0e7ce 165 wait(1);
NicolasV 2:00db719ebd81 166
tony63 0:4e0dfcf0e7ce 167 lcd.cls();
NicolasV 2:00db719ebd81 168 lcd.printf("e=%3.0f",err);
tony63 0:4e0dfcf0e7ce 169 lcd.locate(8,0);
NicolasV 2:00db719ebd81 170 lcd.printf("y=%3.0f",med);
tony63 0:4e0dfcf0e7ce 171 lcd.locate(0,1);
NicolasV 2:00db719ebd81 172 lcd.printf("r=%3.0f",spnum);
tony63 0:4e0dfcf0e7ce 173 lcd.locate(8,1);
NicolasV 2:00db719ebd81 174 lcd.printf("u=%3.0f",pid);
tony63 1:058b8f5c135d 175 wait(1);
tony63 0:4e0dfcf0e7ce 176
NicolasV 2:00db719ebd81 177 int referencia;
NicolasV 2:00db719ebd81 178 int salida;
NicolasV 2:00db719ebd81 179
tony63 0:4e0dfcf0e7ce 180 // CICLO PRINCIPAL CONTROLADOR PID
NicolasV 2:00db719ebd81 181 loop: med = y.read()*999;
NicolasV 2:00db719ebd81 182 err = spnum - med; // Se calcula el error
NicolasV 2:00db719ebd81 183 ap = kpnum*err*0.01f; // Se calcula la acción proporcional
NicolasV 2:00db719ebd81 184 ai += kinum*err*0.01f; // Cálculo de la integral del error
NicolasV 2:00db719ebd81 185 ad = kdnum*(err - err_v)*0.01f; // Cálculo de la acción derivativa
NicolasV 2:00db719ebd81 186 pid = ap + ai + ad;
NicolasV 2:00db719ebd81 187
NicolasV 2:00db719ebd81 188 referencia = (spnum / 999) * 255;
NicolasV 2:00db719ebd81 189 salida = med / 999 * 743 + 256;
NicolasV 2:00db719ebd81 190
NicolasV 2:00db719ebd81 191 if (referencia < 256) { // Debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
NicolasV 2:00db719ebd81 192 GSM.putc(0); // si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
NicolasV 2:00db719ebd81 193 GSM.putc(referencia); // luego la cifra menos significativa
tony63 0:4e0dfcf0e7ce 194 }
NicolasV 2:00db719ebd81 195 /*
NicolasV 2:00db719ebd81 196
NicolasV 2:00db719ebd81 197 if (salida > 255){ // pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
NicolasV 2:00db719ebd81 198 j = salida/256; // calculo la cifra mas significativa
NicolasV 2:00db719ebd81 199 k = salida - j*256; // calculo la cifra menos significativa
NicolasV 2:00db719ebd81 200 GSM.putc(j); // las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
NicolasV 2:00db719ebd81 201 GSM.putc(k); // mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
NicolasV 2:00db719ebd81 202 }
NicolasV 2:00db719ebd81 203
NicolasV 2:00db719ebd81 204
NicolasV 2:00db719ebd81 205 GSM.putc('r'); // Se envía el set-point al módulo Bluetooth.
NicolasV 2:00db719ebd81 206 if (spnum < 256) {
NicolasV 2:00db719ebd81 207 GSM.putc(0);
NicolasV 2:00db719ebd81 208 GSM.putc(spnum);
tony63 0:4e0dfcf0e7ce 209 }
NicolasV 2:00db719ebd81 210 if (spnum > 255){
NicolasV 2:00db719ebd81 211 j = spnum/256;
NicolasV 2:00db719ebd81 212 k = spnum - j*256;
NicolasV 2:00db719ebd81 213 GSM.putc(j);
NicolasV 2:00db719ebd81 214 GSM.putc(k);
NicolasV 2:00db719ebd81 215 }
NicolasV 2:00db719ebd81 216 */
NicolasV 2:00db719ebd81 217
NicolasV 2:00db719ebd81 218 // Se verifica que pid sea positivo:
NicolasV 2:00db719ebd81 219 if (pid <= 0)
NicolasV 2:00db719ebd81 220 pid = 0;
NicolasV 2:00db719ebd81 221 else if (pid > 999)
NicolasV 2:00db719ebd81 222 pid = 999;
tony63 0:4e0dfcf0e7ce 223
tony63 1:058b8f5c135d 224
NicolasV 2:00db719ebd81 225 // Se muestran las variables en LCD:
NicolasV 2:00db719ebd81 226 lcd.locate(3,0);
NicolasV 2:00db719ebd81 227 lcd.printf(" ");
NicolasV 2:00db719ebd81 228 lcd.locate(3,0);
NicolasV 2:00db719ebd81 229 lcd.printf("%3.0f",err);
NicolasV 2:00db719ebd81 230 lcd.locate(11,0);
NicolasV 2:00db719ebd81 231 lcd.printf(" ");
NicolasV 2:00db719ebd81 232 lcd.locate(11,0);
NicolasV 2:00db719ebd81 233 lcd.printf("%3.0f",med);
NicolasV 2:00db719ebd81 234 lcd.locate(3,1);
NicolasV 2:00db719ebd81 235 lcd.printf(" ");
NicolasV 2:00db719ebd81 236 lcd.locate(3,1);
NicolasV 2:00db719ebd81 237 lcd.printf("%d",spnum);
NicolasV 2:00db719ebd81 238 lcd.locate(11,1);
NicolasV 2:00db719ebd81 239 lcd.printf(" ");
NicolasV 2:00db719ebd81 240 lcd.locate(11,1);
NicolasV 2:00db719ebd81 241 lcd.printf("%3.0f",pid);
tony63 1:058b8f5c135d 242
NicolasV 2:00db719ebd81 243 // Normalización de la salida y actualización de variables:
tony63 1:058b8f5c135d 244 err_v = err;
tony63 1:058b8f5c135d 245 o = pid/999;
tony63 1:058b8f5c135d 246 u.write(o);
tony63 1:058b8f5c135d 247
NicolasV 2:00db719ebd81 248 // Se repite el ciclo:
tony63 1:058b8f5c135d 249 wait_ms(300);
NicolasV 2:00db719ebd81 250 goto loop;
tony63 0:4e0dfcf0e7ce 251 }