Controlador PID

Dependencies:   Debounced Pulse1 QEI RTC-DS1307 TextLCD_1 mbed

Fork of grafica_PID_android by Nicolás Villegas Echavarría

/media/uploads/seccastanova/whatsapp_image_2018-06-05_at_10.25.03_am.jpeg

main.cpp

Committer:
NicolasV
Date:
2017-05-10
Revision:
2:00db719ebd81
Parent:
1:058b8f5c135d
Child:
3:2e823b16a4a1

File content as of revision 2:00db719ebd81:

/*
 * Código por: Laura Álvila
 *             Daniela López
 *             Nicolás Villegas            
 */
 
#include "mbed.h"
#include "QEI.h"
#include "TextLCD.h"

Serial GSM(PTE0,PTE1);  // Módulo Bluetooh
TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
QEI encoder (PTA13, PTD5, NC, 1000);


AnalogIn y(PTC2);   // Entrada análoga: Salida del sistema
AnalogOut u(PTE30); // Salida análoga: Señal de control

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

DigitalIn button3(PTC16); // Botón del encoder
DigitalIn button4(PTC17); // Pulsador

// Códigos movimiento del cursor:
int C1=0x0F;
int C2=0x18; // desplaza izquierda
int C3=0x1A; // desplaza derecha
int C4=0x0C; // quito cursor bajo

int cambio = 0, diferencia = 0;
float pid, o, ai, ad, ap, med, err;
float err_v;
int spnum = 0, kinum = 0, kpnum = 0 ,kdnum = 0, pos = 1;
int j,k; 

int main() {
    // Se asigna baudrate y se configura el puerto serie de la USART
    GSM.baud(9600);   
    GSM.format(8,Serial::None,1);
    
    lcd.locate(0,1);
    lcd.printf("**Control PID**");
    wait(3);
    
    lcd.cls();              // Borrar Pantalla
    lcd.writeCommand(C1);   // Escribimos un comando segun el manual del modulo LCD

    lcd.locate(8,0);
    lcd.printf("Kp=%d", kpnum);
    lcd.locate(0,1);
    lcd.printf("Ki=%d", kinum);
    lcd.locate(8,1);
    lcd.printf("Kd=%d", kdnum);
    lcd.locate(0,0);
    lcd.printf("Sp=%d", spnum);

    while(true) {

        diferencia=encoder.getPulses() - cambio;
        cambio=encoder.getPulses();
        
        if (diferencia != 0) {
            switch (pos) {
                case 1:
                    spnum += diferencia;
                    
                    if (spnum >= 999)
                        spnum = 999;
                    else if (spnum < 0)
                        spnum = 0;
                        
                    lcd.locate(3,0);
                    lcd.printf("    ");
                    lcd.locate(3,0);
                    lcd.printf("%d", spnum);
                    break;
                case 2:
                    kpnum += diferencia;
                    
                    if (kpnum >= 999)
                        kpnum = 999;
                    else if (kpnum < 0)
                        kpnum = 0;
                    lcd.locate(11,0);
                    lcd.printf("    ");
                    lcd.locate(11,0);
                    lcd.printf("%d", kpnum);
                    break;
                case 3:
                    kinum += diferencia;
                    
                    if (kinum >= 999)
                        kinum = 999;
                    else if (kinum < 0)
                        kinum = 0;

                    lcd.locate(3,1);
                    lcd.printf("    ");
                    lcd.locate(3,1);
                    lcd.printf("%d", kinum);
                    break;
                case 4:
                    kdnum += diferencia;
                    
                    if (kdnum >= 999)
                        kdnum = 999;
                    else if (kdnum < 0)
                        kdnum = 0;

                    lcd.locate(11,1);
                    lcd.printf("    ");
                    lcd.locate(11,1);
                    lcd.printf("%d", kdnum);
                    break;
                default:
                    break;
            } // Fin switch
        } // Fin if

        if (!button3) { // Cambia la posición de ingreso de parámetros
        
            led3 = !led3;
            
            switch(pos++) {
                case 4:
                    pos = 1;
                    lcd.locate(3,0);
                    lcd.printf("%d", spnum);
                    break;
                case 1:
                    lcd.locate(11,0);
                    lcd.printf("%d", kpnum);
                    break;
                case 2:
                    lcd.locate(3,1);
                    lcd.printf("%d", kinum);
                    break;
                case 3:
                    lcd.locate(11,1);
                    lcd.printf("%d", kdnum);
                    break;
                default:
                    break;
            } // Fin switch
            wait(0.25);
        } // Fin if
              

        if (!button4) {
            break;     //sale del bucle si pisan suiche4
        }
        wait(0.1);        
    }


// Transición
    lcd.writeCommand(C4); // Escribimos un comando segun el manual del módulo LCD para quitar cursor bajo
    lcd.cls(); // Borra la pantalla
    lcd.printf("   GUARDADOS!");
    wait(1);
    lcd.cls();
    lcd.printf("  *** PID ***");
    wait(1);

    lcd.cls();
    lcd.printf("e=%3.0f",err);
    lcd.locate(8,0);
    lcd.printf("y=%3.0f",med);
    lcd.locate(0,1);
    lcd.printf("r=%3.0f",spnum);
    lcd.locate(8,1);
    lcd.printf("u=%3.0f",pid);
    wait(1);

int referencia;
int salida;

// CICLO PRINCIPAL CONTROLADOR PID
loop:   med = y.read()*999;
        err = spnum - med;      // Se calcula el error
        ap = kpnum*err*0.01f;   // Se calcula la acción proporcional
        ai += kinum*err*0.01f;  // Cálculo de la integral del error
        ad = kdnum*(err - err_v)*0.01f; // Cálculo de la acción derivativa
        pid = ap + ai + ad;
        
        referencia = (spnum / 999) * 255;
        salida = med / 999 * 743 + 256;
        
        if (referencia < 256) {        // 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(referencia);       // luego la cifra menos significativa
        }
                /*  
        
        if (salida > 255){         // pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma   
           j = salida/256;         // calculo la cifra mas significativa
           k = salida - j*256;     // calculo la cifra menos significativa
           GSM.putc(j);         // las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
           GSM.putc(k);         // mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
       }
       
 
        GSM.putc('r');          // Se envía el set-point al módulo Bluetooth.
        if (spnum < 256) {    
           GSM.putc(0);
           GSM.putc(spnum);
        }
        if (spnum > 255){  
           j = spnum/256;
           k = spnum - j*256;
           GSM.putc(j);
           GSM.putc(k);
       }
       */
       
        // Se verifica que pid sea positivo:
        if (pid <= 0)
            pid = 0;
        else if (pid > 999)
            pid = 999;

       
        // Se muestran las variables en LCD:
        lcd.locate(3,0);
        lcd.printf("    ");
        lcd.locate(3,0);
        lcd.printf("%3.0f",err);
        lcd.locate(11,0);
        lcd.printf("   ");
        lcd.locate(11,0);
        lcd.printf("%3.0f",med);
        lcd.locate(3,1);
        lcd.printf("   ");
        lcd.locate(3,1);
        lcd.printf("%d",spnum);
        lcd.locate(11,1);
        lcd.printf("   ");
        lcd.locate(11,1);
        lcd.printf("%3.0f",pid);
           
        // Normalización de la salida y actualización de variables:
        err_v = err;
        o = pid/999;
        u.write(o);
        
        // Se repite el ciclo:
        wait_ms(300);
        goto loop;
}