Jaime Osorio / Mbed 2 deprecated Tarea1_PID_Android_Bluetooth

PROCESADORES - 2016-03. TAREA 1. PID+ANDROID+BLUETOOTH

Dependencies:   DebouncedIn QEI TextLCD mbed

Fork of PID_ENCODER_OK by Gustavo Ramirez

Tarea 1: PID + Android + Bluetooth

PROCESADORES - 2016-03

Por:

  • Jaime Alonso Osorio Palacio
  • David Fuertes Chaguezac
  • Wilson Anibal Ortega Andrade
  • John Wilmer Ruiz López

    Recibe por medio de un módulo Bluetooth HC-05 ZS-040 desde un dispositivo Android los parámetros SP, KP, KI y KD. Crea un PID que se prueba sobre una planta de primer orden RC conformado por una resistencia R=11kOhm y un capacitor C=22uF cuya salida y realimentación se conectan por los puertos analógicos determinados en la FRDM KL25Z. Para más información:
  • https://developer.mbed.org/users/tony63/code/PID_ENCODER_OK/
    Reenvía continuamente el error, la medida, el SP y la acción de control hacia el dispositivo Android. Estos también son mostrados en una LCD 16x2.
    Posee depuración via USB en una terminal (se usó Termite 3.2.) para conocer los estados en los que se encuentra el programa y verificar los datos.
    La aplicación para Android se realizó en APP Inventor y se puede descargar en:
  • https://www.dropbox.com/s/gfhh003rq3nknmp/PID_ANDRIOD_ENTEROS.aia?dl=0

    IMÁGENES:

main.cpp@2:278af91e46ff, 2016-11-05 (annotated)

Committer:
jaosoriop
Date:
Sat Nov 05 01:56:06 2016 +0000
Revision:
2:278af91e46ff
Parent:
1:058b8f5c135d 
Recibe por medio de Bluetooth desde un dispositivo Android los par?metros SP, KP, KI y KD, genera un PID cuya salida y realimentaci?n se dan por puertos anal?gicos. Reenv?a continuamente el error, la medida, el SP, la acci?n de control hacia Android.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jaosoriop 2:278af91e46ff 1 #include "mbed.h"
jaosoriop 2:278af91e46ff 2 #include "QEI.h" // Librería del ENCODER GIRATORIO("potenciómetro"+PULSADOR)
jaosoriop 2:278af91e46ff 3 #include "TextLCD.h" // LCD
jaosoriop 2:278af91e46ff 4 #include "DebouncedIn.h"
jaosoriop 2:278af91e46ff 5 #include "stdio.h" //printf, etc
jaosoriop 2:278af91e46ff 6 #include "string.h"
jaosoriop 2:278af91e46ff 7 #include "stdlib.h" // atoi
jaosoriop 2:278af91e46ff 8 #include "stdint.h"
jaosoriop 2:278af91e46ff 9 #include <assert.h>
tony63 0:4e0dfcf0e7ce 10
tony63 0:4e0dfcf0e7ce 11 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
tony63 0:4e0dfcf0e7ce 12 QEI encoder (PTA13, PTD5, NC, 624);
jaosoriop 2:278af91e46ff 13 AnalogIn y(PTB3); //entrada analoga
jaosoriop 2:278af91e46ff 14 AnalogOut u(PTE30); //salida analoga (Imax=1,5mA!!!!)
jaosoriop 2:278af91e46ff 15 DigitalOut LedVerde(LED2);
jaosoriop 2:278af91e46ff 16 DigitalOut LedRojo(LED1);
jaosoriop 2:278af91e46ff 17 DigitalOut LedAzul(LED3);
tony63 0:4e0dfcf0e7ce 18
tony63 0:4e0dfcf0e7ce 19 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
tony63 0:4e0dfcf0e7ce 20
jaosoriop 2:278af91e46ff 21 //****************** CONFIGURACIÓN DE PUERTOS DE COMUNICACIÓN ******************
jaosoriop 2:278af91e46ff 22 Serial BLUETOOTH(PTE0,PTE1); //puertos USART del FRDM para el módulo BLUETOOTH
jaosoriop 2:278af91e46ff 23 Serial pc(USBTX,USBRX);
tony63 0:4e0dfcf0e7ce 24
jaosoriop 2:278af91e46ff 25 //********************************** VARIABLES *********************************
tony63 1:058b8f5c135d 26 int cambio=0, diferencia=0;
tony63 1:058b8f5c135d 27 float pid,o,ai,ad,ap,med,err;
tony63 1:058b8f5c135d 28 float err_v;
tony63 0:4e0dfcf0e7ce 29 int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
jaosoriop 2:278af91e46ff 30 int i,ret;
jaosoriop 2:278af91e46ff 31 int n=0;
jaosoriop 2:278af91e46ff 32 char *ptr;
jaosoriop 2:278af91e46ff 33 char bufferllegada[100];
jaosoriop 2:278af91e46ff 34 char bufferenvio[100];
jaosoriop 2:278af91e46ff 35 Timer t;
tony63 0:4e0dfcf0e7ce 36
jaosoriop 2:278af91e46ff 37 //********************************** FUNCIONES *********************************
jaosoriop 2:278af91e46ff 38 //*********************** LECTURA BUFFER - INTERRUPCIONES **********************
jaosoriop 2:278af91e46ff 39 int readBuffer(char *buffer,int count){
jaosoriop 2:278af91e46ff 40 int i=0;
jaosoriop 2:278af91e46ff 41 t.start(); // start timer
jaosoriop 2:278af91e46ff 42 while(1) {
jaosoriop 2:278af91e46ff 43 while (BLUETOOTH.readable()) {
jaosoriop 2:278af91e46ff 44 char c = BLUETOOTH.getc();
jaosoriop 2:278af91e46ff 45 if (c == '\r' || c == '\n') c = '\0';
jaosoriop 2:278af91e46ff 46 buffer[i++] = c;
jaosoriop 2:278af91e46ff 47 if(i > count)break;
jaosoriop 2:278af91e46ff 48 }
jaosoriop 2:278af91e46ff 49 if(i > count)break;
jaosoriop 2:278af91e46ff 50 if(t.read() > 1) {
jaosoriop 2:278af91e46ff 51 t.stop();
jaosoriop 2:278af91e46ff 52 t.reset();
jaosoriop 2:278af91e46ff 53 break;
jaosoriop 2:278af91e46ff 54 }
jaosoriop 2:278af91e46ff 55 }
jaosoriop 2:278af91e46ff 56 wait(0.5);
jaosoriop 2:278af91e46ff 57 while(BLUETOOTH.readable()) {
jaosoriop 2:278af91e46ff 58 char c = BLUETOOTH.getc();
jaosoriop 2:278af91e46ff 59 }
jaosoriop 2:278af91e46ff 60 return 0;
jaosoriop 2:278af91e46ff 61 }
jaosoriop 2:278af91e46ff 62
jaosoriop 2:278af91e46ff 63 //****************************** BORRADO DE BUFFER *****************************
jaosoriop 2:278af91e46ff 64 void cleanBuffer(char *buffer, int count){
jaosoriop 2:278af91e46ff 65 for(int i=0; i < count; i++) {
jaosoriop 2:278af91e46ff 66 buffer[i] = '\0';
jaosoriop 2:278af91e46ff 67 }
jaosoriop 2:278af91e46ff 68 }
jaosoriop 2:278af91e46ff 69 //******************** SPLIT A STRING BY PARSING STRINGS BY ; ******************
jaosoriop 2:278af91e46ff 70
jaosoriop 2:278af91e46ff 71
jaosoriop 2:278af91e46ff 72 int dtmsplit(char *str, const char *delim, char ***array, int *length ) {
jaosoriop 2:278af91e46ff 73 int i=0;
jaosoriop 2:278af91e46ff 74 char *token;
jaosoriop 2:278af91e46ff 75 char **res = (char **) malloc(0 * sizeof(char *));
jaosoriop 2:278af91e46ff 76
jaosoriop 2:278af91e46ff 77 /* get the first token */
jaosoriop 2:278af91e46ff 78 token = strtok(str, delim);
jaosoriop 2:278af91e46ff 79 while( token != NULL )
jaosoriop 2:278af91e46ff 80 {
jaosoriop 2:278af91e46ff 81 res = (char **) realloc(res, (i + 1) * sizeof(char *));
jaosoriop 2:278af91e46ff 82 res[i] = token;
jaosoriop 2:278af91e46ff 83 i++;
jaosoriop 2:278af91e46ff 84 token = strtok(NULL, delim);
jaosoriop 2:278af91e46ff 85 }
jaosoriop 2:278af91e46ff 86 *array = res;
jaosoriop 2:278af91e46ff 87 *length = i;
jaosoriop 2:278af91e46ff 88 return 1;
jaosoriop 2:278af91e46ff 89 }
jaosoriop 2:278af91e46ff 90
jaosoriop 2:278af91e46ff 91
jaosoriop 2:278af91e46ff 92 //******************************************************************************
jaosoriop 2:278af91e46ff 93 //******************************* CICLO PRINCIPAL ******************************
jaosoriop 2:278af91e46ff 94 //******************************************************************************
tony63 0:4e0dfcf0e7ce 95 int main()
tony63 0:4e0dfcf0e7ce 96 {
jaosoriop 2:278af91e46ff 97 //***************************** BAUDIOS Y BITS *****************************
jaosoriop 2:278af91e46ff 98 BLUETOOTH.baud(9600);
jaosoriop 2:278af91e46ff 99 BLUETOOTH.format(8,Serial::None,1);
jaosoriop 2:278af91e46ff 100
tony63 0:4e0dfcf0e7ce 101 lcd.locate(0,1);
tony63 0:4e0dfcf0e7ce 102 lcd.printf("**Control PID**");
jaosoriop 2:278af91e46ff 103 pc.printf("**Control PID**\r\n");
tony63 0:4e0dfcf0e7ce 104 wait(2);
tony63 0:4e0dfcf0e7ce 105 lcd.cls(); // Borrar Pantalla
tony63 0:4e0dfcf0e7ce 106 lcd.locate(0,0);
jaosoriop 2:278af91e46ff 107 lcd.printf("Esperando datos...");
jaosoriop 2:278af91e46ff 108 pc.printf("Esperando datos...\r\n");
jaosoriop 2:278af91e46ff 109
jaosoriop 2:278af91e46ff 110 while(1){
jaosoriop 2:278af91e46ff 111 if (BLUETOOTH.readable()) {
jaosoriop 2:278af91e46ff 112 readBuffer(bufferllegada,100);
jaosoriop 2:278af91e46ff 113 if(NULL != ( ptr = strstr(bufferllegada,"SP"))){
jaosoriop 2:278af91e46ff 114 printf("Buffer original = [%s] \n\n", bufferllegada);
jaosoriop 2:278af91e46ff 115 //******************** PARTIENDO LA CADENA *********************
jaosoriop 2:278af91e46ff 116 int i;
jaosoriop 2:278af91e46ff 117 char **arr = NULL;
jaosoriop 2:278af91e46ff 118 int count =0;
jaosoriop 2:278af91e46ff 119 int c = dtmsplit(bufferllegada, ";", &arr, &count);
jaosoriop 2:278af91e46ff 120 pc.printf("Found %d tokens.\n", count);
jaosoriop 2:278af91e46ff 121 for (i = 0; i < count; i++){
jaosoriop 2:278af91e46ff 122 pc.printf("string #%d: %s\n", i, arr[i]);
jaosoriop 2:278af91e46ff 123 }
jaosoriop 2:278af91e46ff 124 pc.printf("SP-string=%s\n",arr[1]);
jaosoriop 2:278af91e46ff 125 pc.printf("KP-string=%s\n",arr[3]);
jaosoriop 2:278af91e46ff 126 pc.printf("KI-string=%s\n",arr[5]);
jaosoriop 2:278af91e46ff 127 pc.printf("KD-string=%s\n",arr[7]);
jaosoriop 2:278af91e46ff 128 pc.printf("\n");
jaosoriop 2:278af91e46ff 129 spnum=atoi(arr[1]); //atoi -> Convert string to integer C++
jaosoriop 2:278af91e46ff 130 kpnum=atoi(arr[3]);
jaosoriop 2:278af91e46ff 131 kinum=atoi(arr[5]);
jaosoriop 2:278af91e46ff 132 kdnum=atoi(arr[7]);
jaosoriop 2:278af91e46ff 133 pc.printf("SP=%i\n",spnum);
jaosoriop 2:278af91e46ff 134 pc.printf("KP=%i\n",kpnum);
jaosoriop 2:278af91e46ff 135 pc.printf("KI=%i\n",kinum);
jaosoriop 2:278af91e46ff 136 pc.printf("KD=%i\n",kdnum);
jaosoriop 2:278af91e46ff 137 pc.printf("\n");
jaosoriop 2:278af91e46ff 138 free(arr);
jaosoriop 2:278af91e46ff 139 } //end if(NULL != ( ptr1 = strstr(buffer,"SP"))){
jaosoriop 2:278af91e46ff 140 // My code (tested) in http://stackoverflow.com/questions/9210528/split-string-with-delimiters-in-c
jaosoriop 2:278af91e46ff 141 // http://stackoverflow.com/questions/9054553/atoi-from-string-to-integer-using-char-pointer
jaosoriop 2:278af91e46ff 142 // https://www.tutorialspoint.com/cprogramming/c_pointers.htm
jaosoriop 2:278af91e46ff 143 // Impresión de cada parámetros en LCD
jaosoriop 2:278af91e46ff 144 lcd.cls(); // Borrar Pantalla
jaosoriop 2:278af91e46ff 145 // Imprime SP
jaosoriop 2:278af91e46ff 146 lcd.locate(0,0);
jaosoriop 2:278af91e46ff 147 lcd.printf("Sp=%d",spnum);
jaosoriop 2:278af91e46ff 148 lcd.locate(3,0); // necesariamente (columna, fila)
jaosoriop 2:278af91e46ff 149 lcd.printf(" ");
jaosoriop 2:278af91e46ff 150 lcd.locate(3,0);
jaosoriop 2:278af91e46ff 151 lcd.printf("%d", spnum);
jaosoriop 2:278af91e46ff 152 // Imprime KP
jaosoriop 2:278af91e46ff 153 lcd.locate(8,0);
jaosoriop 2:278af91e46ff 154 lcd.printf("Kp=%d",kpnum);
jaosoriop 2:278af91e46ff 155 lcd.locate(11,0);
jaosoriop 2:278af91e46ff 156 lcd.printf(" ");
jaosoriop 2:278af91e46ff 157 lcd.locate(11,0);
jaosoriop 2:278af91e46ff 158 lcd.printf("%d", kpnum);
jaosoriop 2:278af91e46ff 159 // Imprime KI
jaosoriop 2:278af91e46ff 160 lcd.locate(0,1);
jaosoriop 2:278af91e46ff 161 lcd.printf("Ki=%d",kinum);
jaosoriop 2:278af91e46ff 162 lcd.locate(3,1);
jaosoriop 2:278af91e46ff 163 lcd.printf(" ");
jaosoriop 2:278af91e46ff 164 lcd.locate(3,1);
jaosoriop 2:278af91e46ff 165 lcd.printf("%d", kinum);
jaosoriop 2:278af91e46ff 166 // Imprime KD
jaosoriop 2:278af91e46ff 167 lcd.locate(8,1);
jaosoriop 2:278af91e46ff 168 lcd.printf("Kd=%d",kdnum);
jaosoriop 2:278af91e46ff 169 lcd.locate(11,1);
jaosoriop 2:278af91e46ff 170 lcd.printf(" ");
jaosoriop 2:278af91e46ff 171 lcd.locate(11,1);
jaosoriop 2:278af91e46ff 172 lcd.printf("%d", kdnum);
jaosoriop 2:278af91e46ff 173 } //Fin if (BLUETOOTH.readable()) {
jaosoriop 2:278af91e46ff 174
jaosoriop 2:278af91e46ff 175 if (!button4){
jaosoriop 2:278af91e46ff 176 wait(0.1);
jaosoriop 2:278af91e46ff 177 break; //sale del bucle si pisan suiche4
tony63 0:4e0dfcf0e7ce 178 }
jaosoriop 2:278af91e46ff 179 } // FIN while(1)
jaosoriop 2:278af91e46ff 180
tony63 0:4e0dfcf0e7ce 181
tony63 0:4e0dfcf0e7ce 182 //Transicion
tony63 0:4e0dfcf0e7ce 183 lcd.cls(); //borra la pantalla
tony63 0:4e0dfcf0e7ce 184 lcd.printf(" GUARDADOS!");
jaosoriop 2:278af91e46ff 185 pc.printf(" GUARDADOS!\r\n");
tony63 0:4e0dfcf0e7ce 186 wait(1);
tony63 0:4e0dfcf0e7ce 187 lcd.cls();
tony63 0:4e0dfcf0e7ce 188 lcd.printf(" INICIA EL PID");
jaosoriop 2:278af91e46ff 189 pc.printf(" INICIA EL PID\r\n");
tony63 0:4e0dfcf0e7ce 190 wait(1);
tony63 0:4e0dfcf0e7ce 191 // se imprimen los parches del control *****************************************
tony63 0:4e0dfcf0e7ce 192 lcd.cls();
tony63 1:058b8f5c135d 193 lcd.printf("Er=%3.0f",err);
tony63 0:4e0dfcf0e7ce 194 lcd.locate(8,0);
tony63 1:058b8f5c135d 195 lcd.printf("Me=%3.0f",med);
tony63 0:4e0dfcf0e7ce 196 lcd.locate(0,1);
tony63 1:058b8f5c135d 197 lcd.printf("Sp=%3.0f",spnum);
tony63 0:4e0dfcf0e7ce 198 lcd.locate(8,1);
tony63 1:058b8f5c135d 199 lcd.printf("Co=%3.0f",pid);
tony63 1:058b8f5c135d 200 wait(1);
tony63 0:4e0dfcf0e7ce 201
tony63 0:4e0dfcf0e7ce 202 // CICLO PRINCIPAL CONTROLADOR PID
jaosoriop 2:278af91e46ff 203
jaosoriop 2:278af91e46ff 204 while(1){
jaosoriop 2:278af91e46ff 205 med = y.read()*999;
tony63 0:4e0dfcf0e7ce 206 err = (spnum-med); //se calcula el error
tony63 1:058b8f5c135d 207 ap = kpnum*err*0.01f; //se calcula la accion proporcinal
tony63 1:058b8f5c135d 208 ai =(kinum*err*0.01f)+ai; //calculo de la integral del error
tony63 1:058b8f5c135d 209 ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
jaosoriop 2:278af91e46ff 210 pid = (ap+ai+ad);
jaosoriop 2:278af91e46ff 211
tony63 0:4e0dfcf0e7ce 212 // se verifica que pid sea positivo **************************************
jaosoriop 2:278af91e46ff 213 if(pid<=0){
tony63 0:4e0dfcf0e7ce 214 pid=0;
tony63 0:4e0dfcf0e7ce 215 }
tony63 0:4e0dfcf0e7ce 216 // se verifica que pid sea menor o igual la valor maximo *****************
jaosoriop 2:278af91e46ff 217 if (pid > 999){
tony63 0:4e0dfcf0e7ce 218 pid=999;
tony63 0:4e0dfcf0e7ce 219 }
tony63 0:4e0dfcf0e7ce 220 //se muestran las variables******************************************
tony63 0:4e0dfcf0e7ce 221 lcd.locate(3,0);
tony63 0:4e0dfcf0e7ce 222 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 223 lcd.locate(3,0);
tony63 1:058b8f5c135d 224 lcd.printf("%3.0f",err);
tony63 0:4e0dfcf0e7ce 225 lcd.locate(11,0);
tony63 1:058b8f5c135d 226 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 227 lcd.locate(11,0);
tony63 1:058b8f5c135d 228 lcd.printf("%3.0f",med);
tony63 0:4e0dfcf0e7ce 229 lcd.locate(3,1);
tony63 1:058b8f5c135d 230 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 231 lcd.locate(3,1);
tony63 0:4e0dfcf0e7ce 232 lcd.printf("%d",spnum);
tony63 0:4e0dfcf0e7ce 233 lcd.locate(11,1);
tony63 1:058b8f5c135d 234 lcd.printf(" ");
tony63 0:4e0dfcf0e7ce 235 lcd.locate(11,1);
tony63 1:058b8f5c135d 236 lcd.printf("%3.0f",pid);
tony63 1:058b8f5c135d 237
jaosoriop 2:278af91e46ff 238 /*
jaosoriop 2:278af91e46ff 239 pc.printf("Er=%3.0f\r\n",err);
jaosoriop 2:278af91e46ff 240 pc.printf("Me=%3.0f\r\n",med);
jaosoriop 2:278af91e46ff 241 pc.printf("Sp=%3.0f\r\n",spnum);
jaosoriop 2:278af91e46ff 242 pc.printf("Co=%3.0f\r\n",pid);
jaosoriop 2:278af91e46ff 243 */
tony63 0:4e0dfcf0e7ce 244 //Normalizacion de la salida
tony63 1:058b8f5c135d 245 // se actualizan las variables *******************************************
jaosoriop 2:278af91e46ff 246 // se envia el valor pid a puerto analogico de salida (D/A) **************
tony63 1:058b8f5c135d 247 err_v = err;
tony63 1:058b8f5c135d 248 o = pid/999;
tony63 1:058b8f5c135d 249 u.write(o);
tony63 1:058b8f5c135d 250
tony63 0:4e0dfcf0e7ce 251 // se repite el ciclo
jaosoriop 2:278af91e46ff 252 wait_ms(30);
jaosoriop 2:278af91e46ff 253
jaosoriop 2:278af91e46ff 254 //****************** CONCATENA Y ENVÍA POR BLUETOOTH *******************
jaosoriop 2:278af91e46ff 255 cleanBuffer(bufferllegada,100);
jaosoriop 2:278af91e46ff 256 cleanBuffer(bufferenvio,100);
jaosoriop 2:278af91e46ff 257
jaosoriop 2:278af91e46ff 258 char str1[]="SEND";
jaosoriop 2:278af91e46ff 259 char buf1[10];
jaosoriop 2:278af91e46ff 260 char buf2[10];
jaosoriop 2:278af91e46ff 261 char buf3[10];
jaosoriop 2:278af91e46ff 262 char buf4[10];
jaosoriop 2:278af91e46ff 263
jaosoriop 2:278af91e46ff 264 sprintf(buf1,"%3.0f",err);
jaosoriop 2:278af91e46ff 265 sprintf(buf2,"%3.0f",med);
jaosoriop 2:278af91e46ff 266 sprintf(buf3,"%i",spnum);
jaosoriop 2:278af91e46ff 267 sprintf(buf4,"%3.0f",pid);
jaosoriop 2:278af91e46ff 268 strcpy (bufferenvio,str1);
jaosoriop 2:278af91e46ff 269 strcat (bufferenvio,";");
jaosoriop 2:278af91e46ff 270 strcat (bufferenvio,buf1);
jaosoriop 2:278af91e46ff 271 strcat (bufferenvio,";");
jaosoriop 2:278af91e46ff 272 strcat (bufferenvio,buf2);
jaosoriop 2:278af91e46ff 273 strcat (bufferenvio,";");
jaosoriop 2:278af91e46ff 274 strcat (bufferenvio,buf3);
jaosoriop 2:278af91e46ff 275 strcat (bufferenvio,";");
jaosoriop 2:278af91e46ff 276 strcat (bufferenvio,buf4);
jaosoriop 2:278af91e46ff 277 strcat (bufferenvio,";");
jaosoriop 2:278af91e46ff 278 pc.printf("Datos a enviar= %s\r\n",bufferenvio);
jaosoriop 2:278af91e46ff 279 BLUETOOTH.printf("%s\r\n", bufferenvio);
jaosoriop 2:278af91e46ff 280
jaosoriop 2:278af91e46ff 281 // se envía algo del tipo SEND;XXX.XX;XXX.XX;XXX;XXX.XX
jaosoriop 2:278af91e46ff 282 // al usar SPLIT por ; se tienen 5 elementos
tony63 0:4e0dfcf0e7ce 283 }
jaosoriop 2:278af91e46ff 284 }