This program is designed for control a VTOL plant at Mines Faculty - National University of Colombia.

Dependencies:   DebouncedIn PID QEI SRF05 TextLCD mbed

Fork of SensoresUltrasonicosSRF05 by J Daniel Martinez C

Files at this revision

API Documentation at this revision

Comitter:
stevenjigo
Date:
Sat Jul 30 23:33:01 2016 +0000
Parent:
0:92ceb941f65e
Commit message:
This program is designed for control a VTOL plant at Mines Faculty - National University of Colombia.

Changed in this revision

DebouncedIn.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
TextLCD.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 92ceb941f65e -r ad1952c5a3d7 DebouncedIn.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DebouncedIn.lib	Sat Jul 30 23:33:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/cmorab/code/DebouncedIn/#dc1131de43e8
diff -r 92ceb941f65e -r ad1952c5a3d7 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Sat Jul 30 23:33:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r 92ceb941f65e -r ad1952c5a3d7 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Sat Jul 30 23:33:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 92ceb941f65e -r ad1952c5a3d7 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Sat Jul 30 23:33:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/TextLCD/#308d188a2d3a
diff -r 92ceb941f65e -r ad1952c5a3d7 main.cpp
--- a/main.cpp	Mon Oct 21 16:03:20 2013 +0000
+++ b/main.cpp	Sat Jul 30 23:33:01 2016 +0000
@@ -1,49 +1,654 @@
-#include "mbed.h"
+/*  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"
-              // Trigger  Echo
-SRF05 srfIzquierdo(p11, p12);
-SRF05 srf2Derecho(p13, p14);
-SRF05 srf3Frontal(p15, p16);
+#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.
 
-//motor Izquierdo , atras y adelante
-DigitalOut FernandoAdI(LED1);
-DigitalOut FernandoAtI(LED2);
-//motor derecho , atras y adelante
-DigitalOut FernandoAdD(LED3);
-DigitalOut FernandoAtD(LED4);
+    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
 
-int main() { 
-     while(1) { 
-         printf("Distance = %.1f  %.1f  %.1f \n\r", srfIzquierdo.read(),srf2Derecho.read(),srf3Frontal.read()); 
-          FernandoAdI=0;
-           FernandoAtI=0;
-           FernandoAdD=0;
-           FernandoAtD=0;
-            
-        // Sensor Izquierdo
-         if (srfIzquierdo.read()>15)//distancia de sensro mayor a 15
-         {
-         //motor Izquierdo
-         FernandoAdI=1;   //input 1
-         FernandoAtI=0;   //intput2
-         //motor Derecho
-         FernandoAdD=1;  //input3
-         FernandoAtD=0;  //input 4
-          }
-          else
-          {
-          FernandoAdI=0;
-         FernandoAtI=1;
-         FernandoAdD=0;
-         FernandoAtD=1;
-          }
+    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;
+        } 
         
-          
-          }
-          
-        // wait(0.2); 
-     } 
- 
\ No newline at end of file
+        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