File content as of revision 1:ad1952c5a3d7:

/*  Universidad Nacional de Colombia - Sede Medellín - Facultad de Minas
    2015 - 01
    Este programa realizar el Control de un motor Brushless con el cual se busca controlar 
        la posición angular de un brazo con un pivote con la tarjeta Freescale KL25Z
    Este proyecto se realiza como un PAE con la profesora Ph.D. Eliana Isabel Arango Zuluaga

    Este proyecto es desarrollado por los estudiantes
    Steven Jiménez Gómez - Ingeniería Eléctrica
    Carlos Alberto Arbeláez Giraldo - Ingeniería de Control
*/

// DEFINICIÓN DE LIBRERÍAS A USAR

#include "mbed.h"
#include "TextLCD.h"
#include "DebouncedIn.h"
#include "QEI.h"
#include "SRF05.h"
#include <math.h>

// La librería mbed.h debe agregarse siempre para el funcionamiento del programa
// La librería TextLCD.h es la librería para controlar el LCD, en este caso, 16x2
// La librería DebouncedIn.h evita los rebotes debido a la pulsación de botones
// La librería QEI.h permite manejar un encoder de cuadratura
// La librería US05.h ayuda a leer los datos del sensor de ultrasonido

// CONFIGURACIÓN DE LOS PINES A USAR

Serial PC(USBTX, USBRX); // tx, rx
// Definición del puerto serial con el computador
SRF05 US(PTA4, PTA12);
// Se define el nombre "US" para nombrar el sensor ultrasonido, el primer pin es la conexión del Trigger y el segundo es el Echo
TextLCD LCD(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); 
// La conexión de los pines para la configuración de la pantalla LCD es de la siguiente manera, RS, E, D4-D7
QEI encoder (PTD7, PTD6, NC, 1);
// Define los pines de entrada de un encoder de cuadratura, la conexión dependerá del sentido del encoder
DebouncedIn Pulsador(PTA17);
// Se define la función DebouncedIn dependiendo de la cantidad de botones que se tengan
PwmOut PWM(PTD4);
// Se define la salida PWM para el control del ESC (Electronic Speed controller)

// DEFINICIÓN DE VARIABLES

    DebouncedIn Biz(PTC0);     // Lectura con la libreria antirebote de los botones de mando
    DebouncedIn Bar(PTC3);
    DebouncedIn Bce(PTC4);
    DebouncedIn Bab(PTC5);
    DebouncedIn Bde(PTC6);
    DebouncedIn Bre(PTC7);

float DtyOp1=5, DtyOp2=5, DtyMax=8.6, DtyMaxStd=8, DtyMin=5, CstPIDMin=0, CstPIDMax=5;
float SPMin=15,SPMax=34,KpMin=0,KpMax=10,KiMin=0,KiMax=10,KdMin=0,KdMax=10;
float SP=24.4,Kp=0.01,Ki=0.001,Kd=0,ValPID=0;
float Alt,Error=0,ErrorAnt=0,DeltaP=0,DeltaI=0,DeltaD=0,AcPID=0;
int DelayPID=100;
int Modo=1,Psc=1,ModoPID=1,PscPID=1,ModoS=1;

// La variable Psc define la posición numérica para el cambio del digito del DtyOp1 en el modo de operación 1.

// DEFINICIÓN DE FUNCIONES

    // Esta función satura el valor de las constantes de tipo float entre los valores Vmax y Vmin
    float SaturF(float Val, float Min, float Max){
            if(Val>Max)Val=Max;
            if(Val<Min)Val=Min;
            return Val;
    }

    // Esta función limita el valor de las constantes de tipo float entre los valores Vmax y Vmin
    float LimVarF(float Val, float Min, float Max){
            if(Val>Max)Val=Min;
            if(Val<Min)Val=Max;
            return Val;
    }
    
    // Esta función limita el valor de las constantes de tipo int entre los valores Vmax y Vmin
    int LimVarI(int Val, int Min, int Max){
            if(Val>Max)Val=Min;
            if(Val<Min)Val=Max;
            return Val;
    }

// DEFINICIÓN DE C0MANDOS PARA LCD

int DesIz=0x18;     // Desplazamiento del cursor hacia la izquierda
int DesDe=0x1A;     // Desplazamiento del cursor hacia la derecha
int W1=0x0C;     // Eliminar cursor bajo
int C1=0x0F;     // Iniciar cursor bajo

// INICIO DEL PROGRAMA

int main() {

    LCD.cls();
    LCD.locate(0,0);
    LCD.printf("Control VTOL");
    wait(1);
    
    LCD.cls();
    LCD.locate(0,0);
    LCD.printf("Seleccione modo: ");
            LCD.locate(0,1);
            LCD.printf("1. Lazo abierto");

    PWM.period_ms(20);                      // Definición del periodo de la señal PWM del ESC
    PWM.write(0.05);                        // Definición del ciclo de dureza de la señal.
    while(1) {
    
    // Selección del modo de operación
    
    ModoOp:
    
        if(!Bar){
            Modo--;
            Modo=LimVarI(Modo,1,2);
                while(!Bar);
                goto ModoOpc;
        }
        
        if(!Bab){
            Modo++;
            Modo=LimVarI(Modo,1,2);
                while(!Bab);
                goto ModoOpc;
        }

        if(!Bce){
            LCD.cls();
            LCD.locate(0,0);
            if(Modo==1){
                LCD.printf("Ciclo de dureza:");
                LCD.locate(0,1);
                LCD.printf("%.2f",DtyOp1);
                //LCD.locate(Psc-1,1);
                //LCD.writeCommand(C1);
                    Modo=1;
                    while(!Bce);
                goto ModoOp1;
            }
            if(Modo==2){
                LCD.printf("Modificar:");
                LCD.locate(0,1);
                LCD.printf("1. Set Point");
                    Modo=1;
                    while(!Bce);
                goto MenuPID;
            }
        }
        
    goto ModoOp;
    
//Estas son las 2 opciones para seleccionar el modo de operación, solo se ejecutarán luego de pulsar Bar o Bab. 
//Se hace para evitar copiar dentro de cada if el mismo código.

    ModoOpc:
        if(Modo==1){
            LCD.locate(0,1);
            LCD.printf("1. Lazo abierto");
        }
        if(Modo==2){
            LCD.locate(0,1);
            LCD.printf("2. Lazo cerrado");
        }
    goto ModoOp;
    
    // Modo de operación 1
        
    ModoOp1:
    
        if(!Bre){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Seleccione modo: ");
            LCD.locate(0,1);
            LCD.printf("1. Lazo abierto");
            PWM.write(0.05);
            while(!Bre);
            goto ModoOp;
        } 
        
        if(!Bce){
            //LCD.writeCommand(W1);
            LCD.locate(0,0);
            LCD.printf("                ");
            LCD.locate(0,0);
            if(DtyOp1>5.5)LCD.printf("En operacion");
            if(DtyOp1<5.5)LCD.printf("En pausa");
                Psc=2;
                while(!Bce);
                PWM.write(DtyOp1*0.01);
            goto OperacionM1;
        }
        
        if(!Bde){
            Psc++;
            Psc=LimVarF(Psc,1,3);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            while(!Bde);
        }
        if(!Biz){
            Psc--;
            Psc=LimVarF(Psc,1,3);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            while(!Biz);
        }
    
        if(!Bar){
            if(Psc==1){
                DtyOp1++;
            }
            if(Psc==2){
                DtyOp1=DtyOp1+0.1;
            }
            if(Psc==3){
                DtyOp1=DtyOp1+0.01;
            }
            DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMaxStd);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            while(!Bar);
        }
        if(!Bab){
            if(Psc==1){
                DtyOp1--;
            }
            if(Psc==2){
                DtyOp1=DtyOp1-0.1;
            }
            if(Psc==3){
                DtyOp1=DtyOp1-0.01;
            }
            DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMaxStd);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            while(!Bab);
        }
        
    goto ModoOp1;

OperacionM1:
        Alt=US.read();
        PC.printf(" Altura: %.2f ",Alt);
        PC.printf(" Duty: %.2f \n",DtyOp1);
        
        if(!Bde){
            Psc++;
            Psc=LimVarF(Psc,2,3);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            while(!Bde);
        }
        if(!Biz){
            Psc--;
            Psc=LimVarF(Psc,2,3);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            while(!Biz);
        }
    
        if(!Bar){
            if(Psc==2){
                DtyOp1=DtyOp1+0.1;
            }
            if(Psc==3){
                DtyOp1=DtyOp1+0.01;
            }
            DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMax);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            PWM.write(DtyOp1*0.01);
            while(!Bar);            
        }
        if(!Bab){
            if(Psc==2){
                DtyOp1=DtyOp1-0.1;
            }
            if(Psc==3){
                DtyOp1=DtyOp1-0.01;
            }
            DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMax);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            LCD.locate(Psc-1,1);
            PWM.write(DtyOp1*0.01);
            while(!Bab);
        }
        
        if(!Bre){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Ciclo de dureza:");
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyOp1);
            if(DtyOp1>7.5){
            while(DtyOp1>7.5){
                DtyOp1=DtyOp1-DtyOp1*0.01;
                PWM.write(DtyOp1*0.01);
                wait_ms(400);
            }
            }
            PWM.write(0.05);
            while(!Bre);
            goto ModoOp1;
        }

goto OperacionM1;

    // Modo de operación 2

    MenuPID:
    
        if(!Bar){
            ModoPID--;
            ModoPID=LimVarI(ModoPID,1,6);
                while(!Bar);
                goto PIDOpc;
        }
        
        if(!Bab){
            ModoPID++;
            ModoPID=LimVarI(ModoPID,1,6);
                while(!Bab);
                goto PIDOpc;
        }
        
        if(!Bce){
            LCD.cls();
            LCD.locate(0,0);
            if(ModoPID==1){
                LCD.printf("1. Set Point");
                LCD.locate(0,1);
                LCD.printf("%.2f",SP);
                ValPID=SP;
                CstPIDMin=SPMin;
                CstPIDMax=SPMax;
            }
            if(ModoPID==2){
                LCD.printf("2. Proporcional");
                LCD.locate(0,1);
                LCD.printf("%.2f",Kp);
                ValPID=Kp;
                CstPIDMin=KpMin;
                CstPIDMax=KpMax;
            }
            if(ModoPID==3){
                LCD.printf("3. Integral");
                LCD.locate(0,1);
                LCD.printf("%.2f",Ki);
                ValPID=Ki;
                CstPIDMin=KiMin;
                CstPIDMax=KiMax;
            }
            if(ModoPID==4){
                LCD.printf("4. Derivativo");
                LCD.locate(0,1);
                LCD.printf("%.2f",Kd);
                ValPID=Kd;
                CstPIDMin=KdMin;
                CstPIDMax=KdMax;
            }
            if(ModoPID==5){
                LCD.printf("5. Sensor");
                LCD.locate(0,1);
                LCD.printf("1. SRF05");
                while(!Bce);
                goto Sensado;
            }
            if(ModoPID==6){
                LCD.printf("Iniciando...");
                DtyOp2=7;
//                while(US.read()<SP){
//                    DtyOp2=DtyOp2+0.2;
//                    PWM.write(DtyOp2*0.01);
//                    wait_ms(1000);
//                    }
                LCD.cls();
                LCD.locate(0,0);
                if(SP>=10)LCD.printf("Sp=%.1f",SP);
                if(SP<10)LCD.printf("Sp=%.2f",SP);
                LCD.locate(8,0);
                LCD.printf("Kp=%.2f",Kp);
                LCD.locate(0,1);
                LCD.printf("Ki=%.2f",Ki);
                LCD.locate(8,1);
                LCD.printf("Kd=%.2f",Kd);
                while(!Bce);
                goto OperacionM2;
            }
            while(!Bce);
            goto ValPIDOpc;
        }
        
        if(!Bre){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Seleccione modo: ");
            LCD.locate(0,1);
            LCD.printf("1. Lazo abierto");
            while(!Bre);
            goto ModoOp;
        } 

    goto MenuPID;

//Estas son las 4 opciones para sintonización del PID, solo se ejecutarán luego de pulsar Bar o Bab. 
//Se hace para evitar copiar dentro de cada if el mismo código.

    PIDOpc:
    
            if(ModoPID==1){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("1. Set Point");
            }
            if(ModoPID==2){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("2. Proporcional");
            }
            if(ModoPID==3){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("3. Integral");
            }
            if(ModoPID==4){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("4. Derivativo");
            }
            if(ModoPID==5){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("5. Sensado");
            }
            if(ModoPID==6){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("Iniciar control");
            }
    goto MenuPID;

// Esta programación modifica una variable genérica y luego al sobreescribe sobre la constantes del PID que se haya elegido en MenuPID

    ValPIDOpc:
    
        if(!Bre){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Modificar:");
            LCD.locate(0,1);
            if(ModoPID==1)LCD.printf("1. Set Point");
            if(ModoPID==2)LCD.printf("2. Proporcional");
            if(ModoPID==3)LCD.printf("3. Integral");
            if(ModoPID==4)LCD.printf("4. Derivativo");
            if(ModoPID==5)LCD.printf("5. Sensor");
            while(!Bre);
            goto MenuPID;
        } 
        
        if(!Bce){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Modificar:");
            LCD.locate(0,1);
            if(ModoPID==1){
                LCD.printf("1. Set Point");
                SP=ValPID;
            }
            if(ModoPID==2){
                LCD.printf("2. Proporcional");
                Kp=ValPID;
            }
            if(ModoPID==3){
                LCD.printf("3. Integral");
                Ki=ValPID;
            }
            if(ModoPID==4){
                LCD.printf("4. Derivativo");
                Kd=ValPID;
            }
            while(!Bce);
            goto MenuPID;
        }
        
        if(!Bde){
            PscPID++;
            PscPID=LimVarI(PscPID,1,3);
            while(!Bde);
        }
        if(!Biz){
            PscPID--;
            ValPID=LimVarI(ValPID,1,3);
            while(!Biz);
        }
    
        if(!Bar){
            if(PscPID==1){
                ValPID++;
            }
            if(PscPID==2){
                ValPID=ValPID+0.1;
            }
            if(PscPID==3){
                ValPID=ValPID+0.01;
            }
            ValPID=LimVarF(ValPID,CstPIDMin,CstPIDMax);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",ValPID);
            LCD.locate(PscPID-1,1);
            while(!Bar);
        }
        if(!Bab){
            if(PscPID==1){
                ValPID--;
            }
            if(PscPID==2){
                ValPID=ValPID-0.1;
            }
            if(PscPID==3){
                ValPID=ValPID-0.01;
            }
            ValPID=LimVarF(ValPID,CstPIDMin,CstPIDMax);
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("%.2f",ValPID);
            LCD.locate(PscPID-1,1);
            while(!Bab);
        }
        
    goto ValPIDOpc; 

    Sensado:
    
        if(!Bar){
            ModoS--;
            ModoS=LimVarI(ModoS,1,2);
                while(!Bar);
                goto Sensadoc;
        }
        
        if(!Bab){
            ModoS++;
            ModoS=LimVarI(ModoS,1,2);
                while(!Bab);
                goto Sensadoc;
        }

        if(!Bce){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Modificar:");
            LCD.locate(0,1);
            LCD.printf("1. Set Point");
                Modo=1;
                while(!Bce);
            goto MenuPID;
        }
        
    goto Sensado;
    
//Estas son las 2 opciones para seleccionar el ModoS de operación, solo se ejecutarán luego de pulsar Bar o Bab. 
//Se hace para evitar copiar dentro de cada if el mismo código.

    Sensadoc:
        if(ModoS==1){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("1. SRF05");
        }
        if(ModoS==2){
            LCD.locate(0,1);
            LCD.printf("                ");
            LCD.locate(0,1);
            LCD.printf("2. MPU6050");
        }
    goto Sensado;

OperacionM2:
        
        Alt=US.read();
        PC.printf(" Altura: %.2f ",Alt);
        PC.printf(" Duty: %.2f \n",AcPID);
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("%.2f",Alt);
        Error=SP-Alt;
            LCD.locate(8,0);
            LCD.printf("%.2f",Error);
        DeltaP=Kp*Error;
        DeltaI=DeltaI+Ki*Error;
        DeltaD=Kd*(Error-ErrorAnt);
        AcPID=(DeltaP+DeltaI+DeltaD)*0.05+DtyMaxStd;
        ErrorAnt=Error;
            LCD.locate(8,1);
            LCD.printf("%.2f",AcPID);
        AcPID=SaturF(AcPID,DtyMin,9);
            LCD.locate(0,1);
            LCD.printf("%.2f",AcPID);
            LCD.locate(0,1);
            LCD.printf("%.2f",DtyMin);
        PWM.write(AcPID*0.01);
        wait_ms(DelayPID);
        
        if(!Bre){
            LCD.cls();
            LCD.locate(0,0);
            LCD.printf("Modificar:");
            LCD.locate(0,1);
            LCD.printf("1. Set Point");
                Modo=1;
                PWM.write(0.05);
                while(!Bre);
            goto MenuPID;
        }

goto OperacionM2;

     } // Corchete del While
} // Corchete del main