PID con android

Dependencies:   mbed

Revision:
0:d890d56973d1
Child:
1:e076d163a730
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 24 19:02:20 2019 +0000
@@ -0,0 +1,305 @@
+#include "mbed.h"
+#include "stdio.h"
+#include "string.h"
+#include "stdlib.h"
+
+
+Serial GSM(PTE0,PTE1);  //puertos del FRDM para el modem
+Serial pc(USBTX,USBRX); //puertos del PC
+
+float pid,o,ai,ad,ap,med,err,pwm;
+float err_v;
+float KPNII,KDNII,kdnum=0;
+float kpnum=0,kinum=0,ok=0;
+float spnum=0.0;
+float AP=1;
+float h=0;
+char P;
+
+char buffer[21];// TAMAÑO DEL BUFER
+char bufferB[5];
+char KPC[4],KDC[4];
+char KPCII[4],KDCII[4];
+char *cadena = "";
+char r[]="";
+
+Timer t;   //VALOR DEL TIEMPO
+int count;
+int i = 0;
+int c=0;
+
+
+DigitalOut LedRojo(LED1);
+DigitalOut LedVerde(LED2);
+DigitalOut LedAzul(LED3);
+//PwmOut led(PTA13);
+
+AnalogIn y(PTB3);//entrada analoga
+
+//PwmOut u(PTE30);
+PwmOut led(PTA13);
+
+
+
+
+int readBuffer(char *buffer,int count)   //esta funcion lee un bufer de datos
+{
+    int i=0;
+    t.start();    //CUENTA EL TIEMPO DE CONEXION E INICIA
+    while(1) {
+        while (GSM.readable()) {
+            char c = GSM.getc();
+
+            if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracter inserta $
+            buffer[i++] = c;//mete al bufer el caracter leido
+            if(i > count)break;//sale del loop si ya detecto terminacion
+        }
+        if(i > count)break;
+        if(t.read() > 1) {  //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
+            t.stop();
+            t.reset();
+            break;
+        }
+    }
+    return 0;
+}
+
+void cleanBuffer(char *buffer, int count)  //esta funcion limpia el bufer
+{
+    for(int i=0; i < count; i++) {
+        buffer[i] = '\0';
+    }
+}
+
+
+int main(void)
+{
+LL:
+    AP=0;
+    LedVerde=1;
+    LedRojo=1;
+    LedAzul=1;
+    LedVerde=0;
+    wait(2);   //PRENDE EL LED VERDE POR 2 SEGUNDOS
+    LedVerde=1;
+
+    led.period(0.005f);      // 4 second period
+    led.write(0.40f);      // 50% duty cycle, relative to period
+
+    pc.printf("SI VOLVI");
+    while(1) {
+
+        /////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        LedAzul=0;
+        /*pc.printf(" \n");
+        pc.printf("CONTROLADOR PID (pc) \n");
+        pc.printf("Ingrese el parametro a modificar KP , KD, KI, SP(SetPoint) ) \n" );
+        pc.printf("Escriba M para mirar los parametros del PID \n");
+        pc.printf("Escriba OK para Iniciar PID \n");
+        pc.printf(" \n");
+        ///////////////////////////////////////////////////////////////////////////////////////////////////
+          */
+pc:
+        if (GSM.readable()) {
+            readBuffer(buffer,25);
+
+
+
+            GSM.scanf("%f, %f, %f , %f , %f, %f, %f ", &spnum, &kpnum, &kdnum,&kinum,&AP,&pwm,&h);
+            //GSM.scanf("%f ", &spnum);
+            // pc.printf("buffer= %C  %c\n\r ",buffer);//imprime el cero y el uno
+            pc.printf("buffer= %s  ",buffer);//imprime el cero y el uno
+            //    pc.printf("buffer= %d , %d , %d  ",buffer[0]+buffer[1]+buffer[2]);//imprime el cero y el uno
+
+            pc.printf("\n SP = %f \n", spnum);
+            pc.printf("\n KP = %f \n", kpnum);
+            pc.printf("\n KD = %f \n", kdnum);
+            pc.printf("\n Ki = %f \n", kinum);
+            pc.printf("\n AP = %f \n", AP);
+            pc.printf("\n pwm = %f \n", pwm);
+            pc.printf("\n h = %f \n", h);
+            //cleanBuffer(buffer,15);
+
+
+            pc.printf("ESTOY ACA");
+            //if(ok==1){
+lop1:
+            //cleanBuffer(buffer,15);
+            LedAzul=1;
+            wait(1);
+            LedAzul=0;
+
+
+            med = y.read()*3.3;
+            err = (spnum-med);  //se calcula el error
+            ap = kpnum*err*0.01f;     //se calcula la accion proporcinal
+            ai =(kinum*err*0.01f)+ai;    //calculo de la integral del error
+            ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
+            pid = (ap+ai+ad);
+
+
+            // se verifica que pid sea positivo **************************************
+            if(pid<=0) {
+                pid=0;
+
+            }
+
+            // se verifica que pid sea menor o igual la valor maximo *****************
+            if (pid > 3.3) {
+                pid=3.3;
+
+            }
+
+
+
+
+            pc.printf("ERROR=   %0.01f  ",err);
+            pc.printf("    ");
+            pc.printf("MEDICION=   %0.01f ",med);
+            pc.printf("    ");
+            pc.printf("SETPOINT= %0.01f  ",spnum);
+            pc.printf("    ");
+            pc.printf("PID=  %0.01f \r\n ",pid);
+            pc.printf("    ");
+            pc.printf("pwm= %0.01f  ",pwm);
+            pc.printf("    ");
+
+
+            //Normalizacion de la salida
+            // se actualizan las variables *******************************************
+            err_v = err;
+            o = pid/3.3;
+
+
+            if(AP==1) {
+                AnalogOut u(PTE30);
+                u.write(o);
+                pc.printf("analogo");
+
+            } else if(AP==2) {
+                PwmOut u1(PTE20);
+                u1.write(pwm);
+                pc.printf("digital");
+
+            }
+
+
+            int med2=med*100;
+
+            int p, q, r;
+            if(med2<3.3) {          //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
+                GSM.putc(0);     //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
+                GSM.putc(p);     //luego la cifra menos significativa
+            }
+            if(med2>3.3) {         //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
+                q=med2/3.3;       //calculo la cifra mas significativa
+                r=med2-q*3.3;     //calculo la cifra menos significativa
+                GSM.putc(q);   //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
+                GSM.putc(r);   //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
+            }
+
+            //goto lop1;
+              
+            if (GSM.readable()) {
+                readBuffer(buffer,25);
+                if (strncmp(buffer, "F", 1) == 0) {
+
+                    ok=0;
+                    goto LL;
+
+                    //goto pc;
+                    //cleanBufferB(bufferB,5);
+                //} else if(strncmp(buffer, "P", 1) == 0) {
+                    } else if(h==1) {
+                // pc.printf("%s buffer", buffer);
+                 //GSM.scanf(" %f, %f , %f , %f, %f ,%f,", &spnum, &kpnum, &kdnum,&kinum,&AP,&pwm);
+                  
+pw:
+           
+
+                    if(AP==1) {
+                        AnalogOut u(PTE30);
+                        u.write(o);
+                        pc.printf("analogo");
+
+                    } else if(AP==2) {
+                        PwmOut u1(PTE20);
+                        u1.write(pwm);
+                        pc.printf("digital");
+
+                    }
+
+
+                    //int med2=med*100;
+                    float med2=med;
+                    float med3;
+                   // med3 = y.read();
+                    med3=pwm;
+                    int p, q, r;
+                    if(med3<3.3) {          //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
+                        GSM.putc(0);     //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
+                        GSM.putc(p);     //luego la cifra menos significativa
+                    }
+                    if(med3>3.3) {         //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
+                        //q=med2/3.3;       //calculo la cifra mas significativa
+                        //r=med2-q*3.3;     //calculo la cifra menos significativa
+                        q=med3 ;      //calculo la cifra mas significativa
+                        r=med3-q*3.3;
+                        GSM.putc(q);   //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
+                        GSM.putc(r);   //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
+                    }
+
+
+                    pc.printf("MEDICION=   %0.01f \n",med3);
+                    pc.printf("    ");
+                    pc.printf("pwm= %0.01f  \n",pwm);
+                    pc.printf("    ");
+                    pc.printf("buffer= %s  ",buffer);//imprime el cero y el uno
+                     wait_ms(600);
+
+                   if (GSM.readable()) {
+                readBuffer(buffer,25);
+                if (strncmp(buffer, "F", 1) == 0) {
+
+                    ok=0;
+                    goto LL;
+
+
+                   }
+                   } 
+
+
+
+                //}
+                goto pw;
+               wait_ms(300);
+                
+            } else {
+                wait_ms(300);
+                goto lop1;
+            }
+        } else {
+            wait_ms(300);
+            goto lop1;
+        }
+
+        //  se envia el valor pid a puerto analogico de salida (D/A) **************
+
+        //  se repite el ciclo
+
+
+        // } // DEL IF OK
+        goto pc;
+    } // del if DE LOS DATOS
+    //goto pc;
+
+
+
+    // cleanBuffer(buffer,10);
+
+
+} //WHILE
+
+
+
+}//INT