File content as of revision 0:55be7cc4ef10:

#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");   
        }
    }
}