Diff: main.cpp

Revision:

0:55be7cc4ef10

diff -r 000000000000 -r 55be7cc4ef10 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 14 20:50:38 2019 +0000
@@ -0,0 +1,248 @@
+#include "mbed.h"
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#define BT_RX PTE1
+#define BT_TX PTE0
+
+/*Alvaro Javier Vargas Miranda
+Guillermo Luna Rozo
+Santiago Yepes Montoya
+Juan David Cordoba Giraldo
+Susana Buriticá Mejía*/
+
+DigitalOut lRed(LED1);
+DigitalOut lGrn(LED2);
+DigitalOut lBlu(LED3);
+
+AnalogIn AnInput(PTB0);
+AnalogOut  AnOut(PTE30);
+PwmOut pwmOut(PTE20);
+
+Serial PC(USBTX, USBRX); // tx, rx
+Serial BT(BT_TX, BT_RX); // Serial para el Bluetooth
+
+/* Variables Globales */
+// Parametros del PID sintonizado
+float cKp=1.432f;
+float cKi=25.95f;
+float cKd=0.0f;
+float cRf=1.0f;
+float out,ai,ad,ap,med,err,err_v,uPID;
+int Ts=250;
+bool continuo = true;
+
+char buffer[5]; //Buffer para almacenar los msjs del usuario
+char dbg[100]="";
+
+void cleanBuffer(char *buffer)
+{
+    for(int i=0; i< 5; i++) {
+        buffer[i]='\0';
+    }
+}
+
+void printMenuBT(void)
+{
+    char Menu1[] =   "|********** CONTROLADOR PID ***********|\n";
+    char Menu2[] =   "|*                                    *|\n";
+    char Menu3[] =   "|( P ) -- Ingresar parametros del PID *|\n";
+    char Menu4[] =   "|( I ) -- PID con DCA                 *|\n";
+    char Menu5[] =   "|( W ) -- PID con PWM                 *|\n";
+    char Menu6[] =   "|( S ) -- Apagar la salida            *|\n";
+    char Menu7[] =   "|*                                    *|\n";
+    char Menu8[] =   "|**************************************|\n";
+
+    //Impresion del Menu
+
+    BT.printf("\n%s",Menu1);
+    BT.printf("%s",Menu2);
+    BT.printf("%s",Menu3);
+    BT.printf("%s",Menu4);
+    BT.printf("%s",Menu5);
+    BT.printf("%s",Menu6);
+    BT.printf("%s",Menu7);
+    BT.printf("%s",Menu8);
+}
+
+void enterPIDParameters(void)
+{   
+    bool ready=false;
+    float kp,ki,kd;
+    float ref;
+    char ans[5];
+
+    BT.printf("Ingrese los parametros del controlador.\n\r");
+    while(ready == false) 
+    {
+        BT.printf("PID valores actuales\nKp=%1.4f ki=%1.4f kd=%1.4f Ref=%1.4f\n",cKp,cKi,cKd,cRf);
+        BT.printf("Kp >> ");
+        BT.scanf("%f",&kp);
+        BT.printf("Ki >> ");
+        BT.scanf("%f",&ki);
+        BT.printf("Kd >> ");
+        BT.scanf("%f",&kd);
+        BT.printf("Ref [0 - 3.3]>> ");
+        BT.scanf("%f",&ref);
+
+        BT.printf("Nuevos Valores\nKp=%1.4f Ki=%1.4f Kd=%1.4f Ref=%1.4f\n",kp,ki,kd,ref);
+        BT.printf("Desea continuar? (S/n), (Q) para salir ");
+        BT.scanf("%s",ans);
+        PC.printf("Llego %s \n",ans);
+        
+        if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0 || strncmp(ans,"q",1)==0 ||strncmp(ans,"Q",1)==0) {
+            PC.printf("Entre\n");
+            ready = true;
+        }
+    }
+    if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0){
+        cKp = kp;
+        cKi = ki;
+        cKd = kd;
+        cRf = ref;
+        lRed=0;
+        lGrn=0;
+        BT.printf("PID valores\nKp=%1.2f ki=%1.2f kd=%1.2f Ref=%1.2f\n",cKp,cKi,cKd,cRf);
+    }
+    
+}
+
+// CICLO PRINCIPAL CONTROLADOR PID
+
+void controlPID(){
+    med = AnInput.read()*999;   //Medicion de la salida
+    err = cRf*(999/3.3f) - med; //calculo del error
+    ap = err*cKp*0.01f;         //Calculo accion proporcional
+    ai = ai+(err*cKi*0.01f);    //Calculo accion integral
+    ad = (err-err_v)*(cKd)*0.01f;//Calculo   de la accion derivativa 
+    uPID=ap+ai+ad;              //Calculo de la accion de control
+    if(uPID<=0){uPID=0;}        //Saturador inferior
+    if(uPID>=999){uPID=999;}    //Saturador superior
+    err_v = err;                //Actualizacion de error
+    out=uPID*(3.3f/999);        //Conversion de voltaje a accion de control
+    AnOut.write(out);           //Escritura de accion de control
+    BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out);
+}
+
+void controlPIDpwm(){
+    med = AnInput.read()*999;   //Medicion de la salida
+    err = cRf*(999/3.3f) - med; //calculo del error
+    ap = err*cKp*0.01f;         //Calculo accion proporcional
+    ai = ai+(err*cKi*0.01f);    //Calculo accion integral
+    ad = (err-err_v)*(cKd)*0.01f;//Calculo   de la accion derivativa 
+    uPID=ap+ai+ad;              //Calculo de la accion de control
+    if(uPID<=0){uPID=0;}        //Saturador inferior
+    if(uPID>=999){uPID=999;}    //Saturador superior
+    err_v = err;                //Actualizacion de error
+    out=uPID*(3.3f/999);        //Conversion de voltaje a accion de control
+    pwmOut.write(out);           //Escritura de accion de control
+    BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out);
+}
+
+
+void LeerBuffer(char *buffer)
+{
+    int i= 0;
+    while (BT.readable()) {
+        char c = BT.getc();
+        if (c == '\r' || c == '\n' || c == '\t') c = '$';//si se envia fin de linea o de caracter inserta $
+        buffer[i++] = c;//mete al bufer el caracter leido
+    }
+}
+
+
+void executePIDControl()
+{
+    PC.printf("Entre al PID\n");
+    //Capturamos los parametros del controlador
+    bool run=true;
+    out=0;ai=0;ad=0;ap=0;med=0;err=0;err_v=0;uPID=0;
+    while(run == true) {
+        lRed=1;
+        lGrn=0;
+        if(continuo){controlPID();}
+        else{controlPIDpwm();}
+        wait_ms(Ts);
+        LeerBuffer(buffer);
+        if(buffer[0]=='d' || buffer[0]=='D' || buffer[1]=='d' || buffer[1]=='D') {
+            run = false;
+        }
+        cleanBuffer(buffer);
+    }
+}
+
+
+int main()
+{
+    BT.baud(9600);
+    AnOut.write(0.0f);
+    pwmOut.write(0.0f);
+    lRed = 1;
+    lBlu = 1;
+    for(int i = 0; i<3; i++) {
+        /* Parpadeo de LEDS para indicar el inicio*/
+        lGrn = 0;
+        wait(0.25);
+        lGrn = 1;
+        wait(0.25);
+    }
+    PC.printf("\nProgram Start!\n");
+
+    /*Led en naranja para indicar que esta listo para recibir parametros*/
+    lRed = 0;
+    lGrn = 1;
+    enterPIDParameters();
+
+    while(1) {
+        printMenuBT();
+        BT.printf("\nIngrese una opcion >> ");
+        BT.scanf("%s",buffer);
+        PC.printf("\n Se recibio: %s \n",buffer);
+
+        char msg[10];
+        int mIdx = 0;
+        for (int m = 0; m < 10; m++) {
+            if (buffer[m] != '\n' && buffer[m] != '\r' && buffer[m] != ' ') {
+                msg[mIdx] = buffer[m]; //Guardar los caracteres validos en el mensaje
+                mIdx++;  //Corremos el indice del array de chars
+            }
+        }
+        
+        PC.printf("Msg[1] es <%s> \n",msg);
+        if(strncmp(msg,"P",1)==0 || strncmp(msg,"p",1)==0){
+            enterPIDParameters();
+            BT.printf("Se han ingreso los parametros del PID\n"); 
+        }
+        else if(strncmp(msg,"i",1)==0 || strncmp(msg,"I",1)==0){
+            PC.printf("Llame al PID continuo\n");
+            continuo=true;
+            pwmOut.write(0.0f);
+            executePIDControl();
+            lRed=0;
+            lGrn=0;
+            //BT.printf("Se han ingreso los parametros del PID\n"); 
+        }
+        else if(strncmp(msg,"w",1)==0 || strncmp(msg,"W",1)==0){
+            PC.printf("Llame al PID pwm\n");
+            continuo=false;
+            AnOut.write(0.0f);
+            executePIDControl();
+            lRed=0;
+            lGrn=0;
+            //BT.printf("Se han ingreso los parametros del PID\n"); 
+        }
+        else if(strncmp(msg,"s",1)==0 || strncmp(msg,"S",1)==0){
+            PC.printf("Salida en Bajo\n");
+            lRed=0;
+            lGrn=1;
+            pwmOut.write(0.0f);
+            AnOut.write(0.0f);
+
+            //BT.printf("Se han ingreso los parametros del PID\n"); 
+        }
+        else{
+            BT.printf("El parametro ingresado no es correcto\n");   
+        }
+    }
+}