Steven Jiménez / Mbed 2 deprecated PAE_VTOL

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

main.cpp@1:ad1952c5a3d7, 2016-07-30 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stevenjigo 1:ad1952c5a3d7 1 /* Universidad Nacional de Colombia - Sede Medellín - Facultad de Minas
stevenjigo 1:ad1952c5a3d7 2 2015 - 01
stevenjigo 1:ad1952c5a3d7 3 Este programa realizar el Control de un motor Brushless con el cual se busca controlar
stevenjigo 1:ad1952c5a3d7 4 la posición angular de un brazo con un pivote con la tarjeta Freescale KL25Z
stevenjigo 1:ad1952c5a3d7 5 Este proyecto se realiza como un PAE con la profesora Ph.D. Eliana Isabel Arango Zuluaga
stevenjigo 1:ad1952c5a3d7 6
stevenjigo 1:ad1952c5a3d7 7 Este proyecto es desarrollado por los estudiantes
stevenjigo 1:ad1952c5a3d7 8 Steven Jiménez Gómez - Ingeniería Eléctrica
stevenjigo 1:ad1952c5a3d7 9 Carlos Alberto Arbeláez Giraldo - Ingeniería de Control
stevenjigo 1:ad1952c5a3d7 10 */
stevenjigo 1:ad1952c5a3d7 11
stevenjigo 1:ad1952c5a3d7 12 // DEFINICIÓN DE LIBRERÍAS A USAR
dan_cuspi 0:92ceb941f65e 13
dan_cuspi 0:92ceb941f65e 14 #include "mbed.h"
stevenjigo 1:ad1952c5a3d7 15 #include "TextLCD.h"
stevenjigo 1:ad1952c5a3d7 16 #include "DebouncedIn.h"
stevenjigo 1:ad1952c5a3d7 17 #include "QEI.h"
dan_cuspi 0:92ceb941f65e 18 #include "SRF05.h"
stevenjigo 1:ad1952c5a3d7 19 #include <math.h>
stevenjigo 1:ad1952c5a3d7 20
stevenjigo 1:ad1952c5a3d7 21 // La librería mbed.h debe agregarse siempre para el funcionamiento del programa
stevenjigo 1:ad1952c5a3d7 22 // La librería TextLCD.h es la librería para controlar el LCD, en este caso, 16x2
stevenjigo 1:ad1952c5a3d7 23 // La librería DebouncedIn.h evita los rebotes debido a la pulsación de botones
stevenjigo 1:ad1952c5a3d7 24 // La librería QEI.h permite manejar un encoder de cuadratura
stevenjigo 1:ad1952c5a3d7 25 // La librería US05.h ayuda a leer los datos del sensor de ultrasonido
stevenjigo 1:ad1952c5a3d7 26
stevenjigo 1:ad1952c5a3d7 27 // CONFIGURACIÓN DE LOS PINES A USAR
stevenjigo 1:ad1952c5a3d7 28
stevenjigo 1:ad1952c5a3d7 29 Serial PC(USBTX, USBRX); // tx, rx
stevenjigo 1:ad1952c5a3d7 30 // Definición del puerto serial con el computador
stevenjigo 1:ad1952c5a3d7 31 SRF05 US(PTA4, PTA12);
stevenjigo 1:ad1952c5a3d7 32 // 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
stevenjigo 1:ad1952c5a3d7 33 TextLCD LCD(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5);
stevenjigo 1:ad1952c5a3d7 34 // La conexión de los pines para la configuración de la pantalla LCD es de la siguiente manera, RS, E, D4-D7
stevenjigo 1:ad1952c5a3d7 35 QEI encoder (PTD7, PTD6, NC, 1);
stevenjigo 1:ad1952c5a3d7 36 // Define los pines de entrada de un encoder de cuadratura, la conexión dependerá del sentido del encoder
stevenjigo 1:ad1952c5a3d7 37 DebouncedIn Pulsador(PTA17);
stevenjigo 1:ad1952c5a3d7 38 // Se define la función DebouncedIn dependiendo de la cantidad de botones que se tengan
stevenjigo 1:ad1952c5a3d7 39 PwmOut PWM(PTD4);
stevenjigo 1:ad1952c5a3d7 40 // Se define la salida PWM para el control del ESC (Electronic Speed controller)
stevenjigo 1:ad1952c5a3d7 41
stevenjigo 1:ad1952c5a3d7 42 // DEFINICIÓN DE VARIABLES
stevenjigo 1:ad1952c5a3d7 43
stevenjigo 1:ad1952c5a3d7 44 DebouncedIn Biz(PTC0); // Lectura con la libreria antirebote de los botones de mando
stevenjigo 1:ad1952c5a3d7 45 DebouncedIn Bar(PTC3);
stevenjigo 1:ad1952c5a3d7 46 DebouncedIn Bce(PTC4);
stevenjigo 1:ad1952c5a3d7 47 DebouncedIn Bab(PTC5);
stevenjigo 1:ad1952c5a3d7 48 DebouncedIn Bde(PTC6);
stevenjigo 1:ad1952c5a3d7 49 DebouncedIn Bre(PTC7);
stevenjigo 1:ad1952c5a3d7 50
stevenjigo 1:ad1952c5a3d7 51 float DtyOp1=5, DtyOp2=5, DtyMax=8.6, DtyMaxStd=8, DtyMin=5, CstPIDMin=0, CstPIDMax=5;
stevenjigo 1:ad1952c5a3d7 52 float SPMin=15,SPMax=34,KpMin=0,KpMax=10,KiMin=0,KiMax=10,KdMin=0,KdMax=10;
stevenjigo 1:ad1952c5a3d7 53 float SP=24.4,Kp=0.01,Ki=0.001,Kd=0,ValPID=0;
stevenjigo 1:ad1952c5a3d7 54 float Alt,Error=0,ErrorAnt=0,DeltaP=0,DeltaI=0,DeltaD=0,AcPID=0;
stevenjigo 1:ad1952c5a3d7 55 int DelayPID=100;
stevenjigo 1:ad1952c5a3d7 56 int Modo=1,Psc=1,ModoPID=1,PscPID=1,ModoS=1;
stevenjigo 1:ad1952c5a3d7 57
stevenjigo 1:ad1952c5a3d7 58 // La variable Psc define la posición numérica para el cambio del digito del DtyOp1 en el modo de operación 1.
stevenjigo 1:ad1952c5a3d7 59
stevenjigo 1:ad1952c5a3d7 60 // DEFINICIÓN DE FUNCIONES
stevenjigo 1:ad1952c5a3d7 61
stevenjigo 1:ad1952c5a3d7 62 // Esta función satura el valor de las constantes de tipo float entre los valores Vmax y Vmin
stevenjigo 1:ad1952c5a3d7 63 float SaturF(float Val, float Min, float Max){
stevenjigo 1:ad1952c5a3d7 64 if(Val>Max)Val=Max;
stevenjigo 1:ad1952c5a3d7 65 if(Val<Min)Val=Min;
stevenjigo 1:ad1952c5a3d7 66 return Val;
stevenjigo 1:ad1952c5a3d7 67 }
stevenjigo 1:ad1952c5a3d7 68
stevenjigo 1:ad1952c5a3d7 69 // Esta función limita el valor de las constantes de tipo float entre los valores Vmax y Vmin
stevenjigo 1:ad1952c5a3d7 70 float LimVarF(float Val, float Min, float Max){
stevenjigo 1:ad1952c5a3d7 71 if(Val>Max)Val=Min;
stevenjigo 1:ad1952c5a3d7 72 if(Val<Min)Val=Max;
stevenjigo 1:ad1952c5a3d7 73 return Val;
stevenjigo 1:ad1952c5a3d7 74 }
stevenjigo 1:ad1952c5a3d7 75
stevenjigo 1:ad1952c5a3d7 76 // Esta función limita el valor de las constantes de tipo int entre los valores Vmax y Vmin
stevenjigo 1:ad1952c5a3d7 77 int LimVarI(int Val, int Min, int Max){
stevenjigo 1:ad1952c5a3d7 78 if(Val>Max)Val=Min;
stevenjigo 1:ad1952c5a3d7 79 if(Val<Min)Val=Max;
stevenjigo 1:ad1952c5a3d7 80 return Val;
stevenjigo 1:ad1952c5a3d7 81 }
stevenjigo 1:ad1952c5a3d7 82
stevenjigo 1:ad1952c5a3d7 83 // DEFINICIÓN DE C0MANDOS PARA LCD
dan_cuspi 0:92ceb941f65e 84
stevenjigo 1:ad1952c5a3d7 85 int DesIz=0x18; // Desplazamiento del cursor hacia la izquierda
stevenjigo 1:ad1952c5a3d7 86 int DesDe=0x1A; // Desplazamiento del cursor hacia la derecha
stevenjigo 1:ad1952c5a3d7 87 int W1=0x0C; // Eliminar cursor bajo
stevenjigo 1:ad1952c5a3d7 88 int C1=0x0F; // Iniciar cursor bajo
stevenjigo 1:ad1952c5a3d7 89
stevenjigo 1:ad1952c5a3d7 90 // INICIO DEL PROGRAMA
stevenjigo 1:ad1952c5a3d7 91
stevenjigo 1:ad1952c5a3d7 92 int main() {
stevenjigo 1:ad1952c5a3d7 93
stevenjigo 1:ad1952c5a3d7 94 LCD.cls();
stevenjigo 1:ad1952c5a3d7 95 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 96 LCD.printf("Control VTOL");
stevenjigo 1:ad1952c5a3d7 97 wait(1);
stevenjigo 1:ad1952c5a3d7 98
stevenjigo 1:ad1952c5a3d7 99 LCD.cls();
stevenjigo 1:ad1952c5a3d7 100 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 101 LCD.printf("Seleccione modo: ");
stevenjigo 1:ad1952c5a3d7 102 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 103 LCD.printf("1. Lazo abierto");
stevenjigo 1:ad1952c5a3d7 104
stevenjigo 1:ad1952c5a3d7 105 PWM.period_ms(20); // Definición del periodo de la señal PWM del ESC
stevenjigo 1:ad1952c5a3d7 106 PWM.write(0.05); // Definición del ciclo de dureza de la señal.
stevenjigo 1:ad1952c5a3d7 107 while(1) {
stevenjigo 1:ad1952c5a3d7 108
stevenjigo 1:ad1952c5a3d7 109 // Selección del modo de operación
stevenjigo 1:ad1952c5a3d7 110
stevenjigo 1:ad1952c5a3d7 111 ModoOp:
stevenjigo 1:ad1952c5a3d7 112
stevenjigo 1:ad1952c5a3d7 113 if(!Bar){
stevenjigo 1:ad1952c5a3d7 114 Modo--;
stevenjigo 1:ad1952c5a3d7 115 Modo=LimVarI(Modo,1,2);
stevenjigo 1:ad1952c5a3d7 116 while(!Bar);
stevenjigo 1:ad1952c5a3d7 117 goto ModoOpc;
stevenjigo 1:ad1952c5a3d7 118 }
stevenjigo 1:ad1952c5a3d7 119
stevenjigo 1:ad1952c5a3d7 120 if(!Bab){
stevenjigo 1:ad1952c5a3d7 121 Modo++;
stevenjigo 1:ad1952c5a3d7 122 Modo=LimVarI(Modo,1,2);
stevenjigo 1:ad1952c5a3d7 123 while(!Bab);
stevenjigo 1:ad1952c5a3d7 124 goto ModoOpc;
stevenjigo 1:ad1952c5a3d7 125 }
stevenjigo 1:ad1952c5a3d7 126
stevenjigo 1:ad1952c5a3d7 127 if(!Bce){
stevenjigo 1:ad1952c5a3d7 128 LCD.cls();
stevenjigo 1:ad1952c5a3d7 129 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 130 if(Modo==1){
stevenjigo 1:ad1952c5a3d7 131 LCD.printf("Ciclo de dureza:");
stevenjigo 1:ad1952c5a3d7 132 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 133 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 134 //LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 135 //LCD.writeCommand(C1);
stevenjigo 1:ad1952c5a3d7 136 Modo=1;
stevenjigo 1:ad1952c5a3d7 137 while(!Bce);
stevenjigo 1:ad1952c5a3d7 138 goto ModoOp1;
stevenjigo 1:ad1952c5a3d7 139 }
stevenjigo 1:ad1952c5a3d7 140 if(Modo==2){
stevenjigo 1:ad1952c5a3d7 141 LCD.printf("Modificar:");
stevenjigo 1:ad1952c5a3d7 142 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 143 LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 144 Modo=1;
stevenjigo 1:ad1952c5a3d7 145 while(!Bce);
stevenjigo 1:ad1952c5a3d7 146 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 147 }
stevenjigo 1:ad1952c5a3d7 148 }
stevenjigo 1:ad1952c5a3d7 149
stevenjigo 1:ad1952c5a3d7 150 goto ModoOp;
stevenjigo 1:ad1952c5a3d7 151
stevenjigo 1:ad1952c5a3d7 152 //Estas son las 2 opciones para seleccionar el modo de operación, solo se ejecutarán luego de pulsar Bar o Bab.
stevenjigo 1:ad1952c5a3d7 153 //Se hace para evitar copiar dentro de cada if el mismo código.
dan_cuspi 0:92ceb941f65e 154
stevenjigo 1:ad1952c5a3d7 155 ModoOpc:
stevenjigo 1:ad1952c5a3d7 156 if(Modo==1){
stevenjigo 1:ad1952c5a3d7 157 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 158 LCD.printf("1. Lazo abierto");
stevenjigo 1:ad1952c5a3d7 159 }
stevenjigo 1:ad1952c5a3d7 160 if(Modo==2){
stevenjigo 1:ad1952c5a3d7 161 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 162 LCD.printf("2. Lazo cerrado");
stevenjigo 1:ad1952c5a3d7 163 }
stevenjigo 1:ad1952c5a3d7 164 goto ModoOp;
stevenjigo 1:ad1952c5a3d7 165
stevenjigo 1:ad1952c5a3d7 166 // Modo de operación 1
stevenjigo 1:ad1952c5a3d7 167
stevenjigo 1:ad1952c5a3d7 168 ModoOp1:
stevenjigo 1:ad1952c5a3d7 169
stevenjigo 1:ad1952c5a3d7 170 if(!Bre){
stevenjigo 1:ad1952c5a3d7 171 LCD.cls();
stevenjigo 1:ad1952c5a3d7 172 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 173 LCD.printf("Seleccione modo: ");
stevenjigo 1:ad1952c5a3d7 174 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 175 LCD.printf("1. Lazo abierto");
stevenjigo 1:ad1952c5a3d7 176 PWM.write(0.05);
stevenjigo 1:ad1952c5a3d7 177 while(!Bre);
stevenjigo 1:ad1952c5a3d7 178 goto ModoOp;
stevenjigo 1:ad1952c5a3d7 179 }
stevenjigo 1:ad1952c5a3d7 180
stevenjigo 1:ad1952c5a3d7 181 if(!Bce){
stevenjigo 1:ad1952c5a3d7 182 //LCD.writeCommand(W1);
stevenjigo 1:ad1952c5a3d7 183 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 184 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 185 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 186 if(DtyOp1>5.5)LCD.printf("En operacion");
stevenjigo 1:ad1952c5a3d7 187 if(DtyOp1<5.5)LCD.printf("En pausa");
stevenjigo 1:ad1952c5a3d7 188 Psc=2;
stevenjigo 1:ad1952c5a3d7 189 while(!Bce);
stevenjigo 1:ad1952c5a3d7 190 PWM.write(DtyOp1*0.01);
stevenjigo 1:ad1952c5a3d7 191 goto OperacionM1;
stevenjigo 1:ad1952c5a3d7 192 }
stevenjigo 1:ad1952c5a3d7 193
stevenjigo 1:ad1952c5a3d7 194 if(!Bde){
stevenjigo 1:ad1952c5a3d7 195 Psc++;
stevenjigo 1:ad1952c5a3d7 196 Psc=LimVarF(Psc,1,3);
stevenjigo 1:ad1952c5a3d7 197 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 198 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 199 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 200 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 201 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 202 while(!Bde);
stevenjigo 1:ad1952c5a3d7 203 }
stevenjigo 1:ad1952c5a3d7 204 if(!Biz){
stevenjigo 1:ad1952c5a3d7 205 Psc--;
stevenjigo 1:ad1952c5a3d7 206 Psc=LimVarF(Psc,1,3);
stevenjigo 1:ad1952c5a3d7 207 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 208 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 209 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 210 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 211 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 212 while(!Biz);
stevenjigo 1:ad1952c5a3d7 213 }
stevenjigo 1:ad1952c5a3d7 214
stevenjigo 1:ad1952c5a3d7 215 if(!Bar){
stevenjigo 1:ad1952c5a3d7 216 if(Psc==1){
stevenjigo 1:ad1952c5a3d7 217 DtyOp1++;
stevenjigo 1:ad1952c5a3d7 218 }
stevenjigo 1:ad1952c5a3d7 219 if(Psc==2){
stevenjigo 1:ad1952c5a3d7 220 DtyOp1=DtyOp1+0.1;
stevenjigo 1:ad1952c5a3d7 221 }
stevenjigo 1:ad1952c5a3d7 222 if(Psc==3){
stevenjigo 1:ad1952c5a3d7 223 DtyOp1=DtyOp1+0.01;
stevenjigo 1:ad1952c5a3d7 224 }
stevenjigo 1:ad1952c5a3d7 225 DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMaxStd);
stevenjigo 1:ad1952c5a3d7 226 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 227 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 228 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 229 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 230 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 231 while(!Bar);
stevenjigo 1:ad1952c5a3d7 232 }
stevenjigo 1:ad1952c5a3d7 233 if(!Bab){
stevenjigo 1:ad1952c5a3d7 234 if(Psc==1){
stevenjigo 1:ad1952c5a3d7 235 DtyOp1--;
stevenjigo 1:ad1952c5a3d7 236 }
stevenjigo 1:ad1952c5a3d7 237 if(Psc==2){
stevenjigo 1:ad1952c5a3d7 238 DtyOp1=DtyOp1-0.1;
stevenjigo 1:ad1952c5a3d7 239 }
stevenjigo 1:ad1952c5a3d7 240 if(Psc==3){
stevenjigo 1:ad1952c5a3d7 241 DtyOp1=DtyOp1-0.01;
stevenjigo 1:ad1952c5a3d7 242 }
stevenjigo 1:ad1952c5a3d7 243 DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMaxStd);
stevenjigo 1:ad1952c5a3d7 244 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 245 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 246 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 247 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 248 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 249 while(!Bab);
stevenjigo 1:ad1952c5a3d7 250 }
stevenjigo 1:ad1952c5a3d7 251
stevenjigo 1:ad1952c5a3d7 252 goto ModoOp1;
stevenjigo 1:ad1952c5a3d7 253
stevenjigo 1:ad1952c5a3d7 254 OperacionM1:
stevenjigo 1:ad1952c5a3d7 255 Alt=US.read();
stevenjigo 1:ad1952c5a3d7 256 PC.printf(" Altura: %.2f ",Alt);
stevenjigo 1:ad1952c5a3d7 257 PC.printf(" Duty: %.2f \n",DtyOp1);
stevenjigo 1:ad1952c5a3d7 258
stevenjigo 1:ad1952c5a3d7 259 if(!Bde){
stevenjigo 1:ad1952c5a3d7 260 Psc++;
stevenjigo 1:ad1952c5a3d7 261 Psc=LimVarF(Psc,2,3);
stevenjigo 1:ad1952c5a3d7 262 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 263 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 264 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 265 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 266 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 267 while(!Bde);
stevenjigo 1:ad1952c5a3d7 268 }
stevenjigo 1:ad1952c5a3d7 269 if(!Biz){
stevenjigo 1:ad1952c5a3d7 270 Psc--;
stevenjigo 1:ad1952c5a3d7 271 Psc=LimVarF(Psc,2,3);
stevenjigo 1:ad1952c5a3d7 272 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 273 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 274 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 275 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 276 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 277 while(!Biz);
stevenjigo 1:ad1952c5a3d7 278 }
stevenjigo 1:ad1952c5a3d7 279
stevenjigo 1:ad1952c5a3d7 280 if(!Bar){
stevenjigo 1:ad1952c5a3d7 281 if(Psc==2){
stevenjigo 1:ad1952c5a3d7 282 DtyOp1=DtyOp1+0.1;
stevenjigo 1:ad1952c5a3d7 283 }
stevenjigo 1:ad1952c5a3d7 284 if(Psc==3){
stevenjigo 1:ad1952c5a3d7 285 DtyOp1=DtyOp1+0.01;
stevenjigo 1:ad1952c5a3d7 286 }
stevenjigo 1:ad1952c5a3d7 287 DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMax);
stevenjigo 1:ad1952c5a3d7 288 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 289 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 290 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 291 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 292 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 293 PWM.write(DtyOp1*0.01);
stevenjigo 1:ad1952c5a3d7 294 while(!Bar);
stevenjigo 1:ad1952c5a3d7 295 }
stevenjigo 1:ad1952c5a3d7 296 if(!Bab){
stevenjigo 1:ad1952c5a3d7 297 if(Psc==2){
stevenjigo 1:ad1952c5a3d7 298 DtyOp1=DtyOp1-0.1;
stevenjigo 1:ad1952c5a3d7 299 }
stevenjigo 1:ad1952c5a3d7 300 if(Psc==3){
stevenjigo 1:ad1952c5a3d7 301 DtyOp1=DtyOp1-0.01;
stevenjigo 1:ad1952c5a3d7 302 }
stevenjigo 1:ad1952c5a3d7 303 DtyOp1=LimVarF(DtyOp1,DtyMin,DtyMax);
stevenjigo 1:ad1952c5a3d7 304 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 305 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 306 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 307 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 308 LCD.locate(Psc-1,1);
stevenjigo 1:ad1952c5a3d7 309 PWM.write(DtyOp1*0.01);
stevenjigo 1:ad1952c5a3d7 310 while(!Bab);
stevenjigo 1:ad1952c5a3d7 311 }
stevenjigo 1:ad1952c5a3d7 312
stevenjigo 1:ad1952c5a3d7 313 if(!Bre){
stevenjigo 1:ad1952c5a3d7 314 LCD.cls();
stevenjigo 1:ad1952c5a3d7 315 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 316 LCD.printf("Ciclo de dureza:");
stevenjigo 1:ad1952c5a3d7 317 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 318 LCD.printf("%.2f",DtyOp1);
stevenjigo 1:ad1952c5a3d7 319 if(DtyOp1>7.5){
stevenjigo 1:ad1952c5a3d7 320 while(DtyOp1>7.5){
stevenjigo 1:ad1952c5a3d7 321 DtyOp1=DtyOp1-DtyOp1*0.01;
stevenjigo 1:ad1952c5a3d7 322 PWM.write(DtyOp1*0.01);
stevenjigo 1:ad1952c5a3d7 323 wait_ms(400);
stevenjigo 1:ad1952c5a3d7 324 }
stevenjigo 1:ad1952c5a3d7 325 }
stevenjigo 1:ad1952c5a3d7 326 PWM.write(0.05);
stevenjigo 1:ad1952c5a3d7 327 while(!Bre);
stevenjigo 1:ad1952c5a3d7 328 goto ModoOp1;
stevenjigo 1:ad1952c5a3d7 329 }
stevenjigo 1:ad1952c5a3d7 330
stevenjigo 1:ad1952c5a3d7 331 goto OperacionM1;
stevenjigo 1:ad1952c5a3d7 332
stevenjigo 1:ad1952c5a3d7 333 // Modo de operación 2
dan_cuspi 0:92ceb941f65e 334
stevenjigo 1:ad1952c5a3d7 335 MenuPID:
stevenjigo 1:ad1952c5a3d7 336
stevenjigo 1:ad1952c5a3d7 337 if(!Bar){
stevenjigo 1:ad1952c5a3d7 338 ModoPID--;
stevenjigo 1:ad1952c5a3d7 339 ModoPID=LimVarI(ModoPID,1,6);
stevenjigo 1:ad1952c5a3d7 340 while(!Bar);
stevenjigo 1:ad1952c5a3d7 341 goto PIDOpc;
stevenjigo 1:ad1952c5a3d7 342 }
stevenjigo 1:ad1952c5a3d7 343
stevenjigo 1:ad1952c5a3d7 344 if(!Bab){
stevenjigo 1:ad1952c5a3d7 345 ModoPID++;
stevenjigo 1:ad1952c5a3d7 346 ModoPID=LimVarI(ModoPID,1,6);
stevenjigo 1:ad1952c5a3d7 347 while(!Bab);
stevenjigo 1:ad1952c5a3d7 348 goto PIDOpc;
stevenjigo 1:ad1952c5a3d7 349 }
stevenjigo 1:ad1952c5a3d7 350
stevenjigo 1:ad1952c5a3d7 351 if(!Bce){
stevenjigo 1:ad1952c5a3d7 352 LCD.cls();
stevenjigo 1:ad1952c5a3d7 353 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 354 if(ModoPID==1){
stevenjigo 1:ad1952c5a3d7 355 LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 356 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 357 LCD.printf("%.2f",SP);
stevenjigo 1:ad1952c5a3d7 358 ValPID=SP;
stevenjigo 1:ad1952c5a3d7 359 CstPIDMin=SPMin;
stevenjigo 1:ad1952c5a3d7 360 CstPIDMax=SPMax;
stevenjigo 1:ad1952c5a3d7 361 }
stevenjigo 1:ad1952c5a3d7 362 if(ModoPID==2){
stevenjigo 1:ad1952c5a3d7 363 LCD.printf("2. Proporcional");
stevenjigo 1:ad1952c5a3d7 364 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 365 LCD.printf("%.2f",Kp);
stevenjigo 1:ad1952c5a3d7 366 ValPID=Kp;
stevenjigo 1:ad1952c5a3d7 367 CstPIDMin=KpMin;
stevenjigo 1:ad1952c5a3d7 368 CstPIDMax=KpMax;
stevenjigo 1:ad1952c5a3d7 369 }
stevenjigo 1:ad1952c5a3d7 370 if(ModoPID==3){
stevenjigo 1:ad1952c5a3d7 371 LCD.printf("3. Integral");
stevenjigo 1:ad1952c5a3d7 372 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 373 LCD.printf("%.2f",Ki);
stevenjigo 1:ad1952c5a3d7 374 ValPID=Ki;
stevenjigo 1:ad1952c5a3d7 375 CstPIDMin=KiMin;
stevenjigo 1:ad1952c5a3d7 376 CstPIDMax=KiMax;
stevenjigo 1:ad1952c5a3d7 377 }
stevenjigo 1:ad1952c5a3d7 378 if(ModoPID==4){
stevenjigo 1:ad1952c5a3d7 379 LCD.printf("4. Derivativo");
stevenjigo 1:ad1952c5a3d7 380 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 381 LCD.printf("%.2f",Kd);
stevenjigo 1:ad1952c5a3d7 382 ValPID=Kd;
stevenjigo 1:ad1952c5a3d7 383 CstPIDMin=KdMin;
stevenjigo 1:ad1952c5a3d7 384 CstPIDMax=KdMax;
stevenjigo 1:ad1952c5a3d7 385 }
stevenjigo 1:ad1952c5a3d7 386 if(ModoPID==5){
stevenjigo 1:ad1952c5a3d7 387 LCD.printf("5. Sensor");
stevenjigo 1:ad1952c5a3d7 388 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 389 LCD.printf("1. SRF05");
stevenjigo 1:ad1952c5a3d7 390 while(!Bce);
stevenjigo 1:ad1952c5a3d7 391 goto Sensado;
stevenjigo 1:ad1952c5a3d7 392 }
stevenjigo 1:ad1952c5a3d7 393 if(ModoPID==6){
stevenjigo 1:ad1952c5a3d7 394 LCD.printf("Iniciando...");
stevenjigo 1:ad1952c5a3d7 395 DtyOp2=7;
stevenjigo 1:ad1952c5a3d7 396 // while(US.read()<SP){
stevenjigo 1:ad1952c5a3d7 397 // DtyOp2=DtyOp2+0.2;
stevenjigo 1:ad1952c5a3d7 398 // PWM.write(DtyOp2*0.01);
stevenjigo 1:ad1952c5a3d7 399 // wait_ms(1000);
stevenjigo 1:ad1952c5a3d7 400 // }
stevenjigo 1:ad1952c5a3d7 401 LCD.cls();
stevenjigo 1:ad1952c5a3d7 402 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 403 if(SP>=10)LCD.printf("Sp=%.1f",SP);
stevenjigo 1:ad1952c5a3d7 404 if(SP<10)LCD.printf("Sp=%.2f",SP);
stevenjigo 1:ad1952c5a3d7 405 LCD.locate(8,0);
stevenjigo 1:ad1952c5a3d7 406 LCD.printf("Kp=%.2f",Kp);
stevenjigo 1:ad1952c5a3d7 407 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 408 LCD.printf("Ki=%.2f",Ki);
stevenjigo 1:ad1952c5a3d7 409 LCD.locate(8,1);
stevenjigo 1:ad1952c5a3d7 410 LCD.printf("Kd=%.2f",Kd);
stevenjigo 1:ad1952c5a3d7 411 while(!Bce);
stevenjigo 1:ad1952c5a3d7 412 goto OperacionM2;
stevenjigo 1:ad1952c5a3d7 413 }
stevenjigo 1:ad1952c5a3d7 414 while(!Bce);
stevenjigo 1:ad1952c5a3d7 415 goto ValPIDOpc;
stevenjigo 1:ad1952c5a3d7 416 }
stevenjigo 1:ad1952c5a3d7 417
stevenjigo 1:ad1952c5a3d7 418 if(!Bre){
stevenjigo 1:ad1952c5a3d7 419 LCD.cls();
stevenjigo 1:ad1952c5a3d7 420 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 421 LCD.printf("Seleccione modo: ");
stevenjigo 1:ad1952c5a3d7 422 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 423 LCD.printf("1. Lazo abierto");
stevenjigo 1:ad1952c5a3d7 424 while(!Bre);
stevenjigo 1:ad1952c5a3d7 425 goto ModoOp;
stevenjigo 1:ad1952c5a3d7 426 }
stevenjigo 1:ad1952c5a3d7 427
stevenjigo 1:ad1952c5a3d7 428 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 429
stevenjigo 1:ad1952c5a3d7 430 //Estas son las 4 opciones para sintonización del PID, solo se ejecutarán luego de pulsar Bar o Bab.
stevenjigo 1:ad1952c5a3d7 431 //Se hace para evitar copiar dentro de cada if el mismo código.
stevenjigo 1:ad1952c5a3d7 432
stevenjigo 1:ad1952c5a3d7 433 PIDOpc:
stevenjigo 1:ad1952c5a3d7 434
stevenjigo 1:ad1952c5a3d7 435 if(ModoPID==1){
stevenjigo 1:ad1952c5a3d7 436 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 437 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 438 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 439 LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 440 }
stevenjigo 1:ad1952c5a3d7 441 if(ModoPID==2){
stevenjigo 1:ad1952c5a3d7 442 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 443 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 444 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 445 LCD.printf("2. Proporcional");
stevenjigo 1:ad1952c5a3d7 446 }
stevenjigo 1:ad1952c5a3d7 447 if(ModoPID==3){
stevenjigo 1:ad1952c5a3d7 448 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 449 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 450 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 451 LCD.printf("3. Integral");
stevenjigo 1:ad1952c5a3d7 452 }
stevenjigo 1:ad1952c5a3d7 453 if(ModoPID==4){
stevenjigo 1:ad1952c5a3d7 454 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 455 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 456 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 457 LCD.printf("4. Derivativo");
stevenjigo 1:ad1952c5a3d7 458 }
stevenjigo 1:ad1952c5a3d7 459 if(ModoPID==5){
stevenjigo 1:ad1952c5a3d7 460 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 461 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 462 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 463 LCD.printf("5. Sensado");
stevenjigo 1:ad1952c5a3d7 464 }
stevenjigo 1:ad1952c5a3d7 465 if(ModoPID==6){
stevenjigo 1:ad1952c5a3d7 466 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 467 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 468 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 469 LCD.printf("Iniciar control");
stevenjigo 1:ad1952c5a3d7 470 }
stevenjigo 1:ad1952c5a3d7 471 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 472
stevenjigo 1:ad1952c5a3d7 473 // Esta programación modifica una variable genérica y luego al sobreescribe sobre la constantes del PID que se haya elegido en MenuPID
stevenjigo 1:ad1952c5a3d7 474
stevenjigo 1:ad1952c5a3d7 475 ValPIDOpc:
stevenjigo 1:ad1952c5a3d7 476
stevenjigo 1:ad1952c5a3d7 477 if(!Bre){
stevenjigo 1:ad1952c5a3d7 478 LCD.cls();
stevenjigo 1:ad1952c5a3d7 479 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 480 LCD.printf("Modificar:");
stevenjigo 1:ad1952c5a3d7 481 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 482 if(ModoPID==1)LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 483 if(ModoPID==2)LCD.printf("2. Proporcional");
stevenjigo 1:ad1952c5a3d7 484 if(ModoPID==3)LCD.printf("3. Integral");
stevenjigo 1:ad1952c5a3d7 485 if(ModoPID==4)LCD.printf("4. Derivativo");
stevenjigo 1:ad1952c5a3d7 486 if(ModoPID==5)LCD.printf("5. Sensor");
stevenjigo 1:ad1952c5a3d7 487 while(!Bre);
stevenjigo 1:ad1952c5a3d7 488 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 489 }
dan_cuspi 0:92ceb941f65e 490
stevenjigo 1:ad1952c5a3d7 491 if(!Bce){
stevenjigo 1:ad1952c5a3d7 492 LCD.cls();
stevenjigo 1:ad1952c5a3d7 493 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 494 LCD.printf("Modificar:");
stevenjigo 1:ad1952c5a3d7 495 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 496 if(ModoPID==1){
stevenjigo 1:ad1952c5a3d7 497 LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 498 SP=ValPID;
stevenjigo 1:ad1952c5a3d7 499 }
stevenjigo 1:ad1952c5a3d7 500 if(ModoPID==2){
stevenjigo 1:ad1952c5a3d7 501 LCD.printf("2. Proporcional");
stevenjigo 1:ad1952c5a3d7 502 Kp=ValPID;
stevenjigo 1:ad1952c5a3d7 503 }
stevenjigo 1:ad1952c5a3d7 504 if(ModoPID==3){
stevenjigo 1:ad1952c5a3d7 505 LCD.printf("3. Integral");
stevenjigo 1:ad1952c5a3d7 506 Ki=ValPID;
stevenjigo 1:ad1952c5a3d7 507 }
stevenjigo 1:ad1952c5a3d7 508 if(ModoPID==4){
stevenjigo 1:ad1952c5a3d7 509 LCD.printf("4. Derivativo");
stevenjigo 1:ad1952c5a3d7 510 Kd=ValPID;
stevenjigo 1:ad1952c5a3d7 511 }
stevenjigo 1:ad1952c5a3d7 512 while(!Bce);
stevenjigo 1:ad1952c5a3d7 513 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 514 }
stevenjigo 1:ad1952c5a3d7 515
stevenjigo 1:ad1952c5a3d7 516 if(!Bde){
stevenjigo 1:ad1952c5a3d7 517 PscPID++;
stevenjigo 1:ad1952c5a3d7 518 PscPID=LimVarI(PscPID,1,3);
stevenjigo 1:ad1952c5a3d7 519 while(!Bde);
stevenjigo 1:ad1952c5a3d7 520 }
stevenjigo 1:ad1952c5a3d7 521 if(!Biz){
stevenjigo 1:ad1952c5a3d7 522 PscPID--;
stevenjigo 1:ad1952c5a3d7 523 ValPID=LimVarI(ValPID,1,3);
stevenjigo 1:ad1952c5a3d7 524 while(!Biz);
stevenjigo 1:ad1952c5a3d7 525 }
stevenjigo 1:ad1952c5a3d7 526
stevenjigo 1:ad1952c5a3d7 527 if(!Bar){
stevenjigo 1:ad1952c5a3d7 528 if(PscPID==1){
stevenjigo 1:ad1952c5a3d7 529 ValPID++;
stevenjigo 1:ad1952c5a3d7 530 }
stevenjigo 1:ad1952c5a3d7 531 if(PscPID==2){
stevenjigo 1:ad1952c5a3d7 532 ValPID=ValPID+0.1;
stevenjigo 1:ad1952c5a3d7 533 }
stevenjigo 1:ad1952c5a3d7 534 if(PscPID==3){
stevenjigo 1:ad1952c5a3d7 535 ValPID=ValPID+0.01;
stevenjigo 1:ad1952c5a3d7 536 }
stevenjigo 1:ad1952c5a3d7 537 ValPID=LimVarF(ValPID,CstPIDMin,CstPIDMax);
stevenjigo 1:ad1952c5a3d7 538 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 539 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 540 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 541 LCD.printf("%.2f",ValPID);
stevenjigo 1:ad1952c5a3d7 542 LCD.locate(PscPID-1,1);
stevenjigo 1:ad1952c5a3d7 543 while(!Bar);
stevenjigo 1:ad1952c5a3d7 544 }
stevenjigo 1:ad1952c5a3d7 545 if(!Bab){
stevenjigo 1:ad1952c5a3d7 546 if(PscPID==1){
stevenjigo 1:ad1952c5a3d7 547 ValPID--;
stevenjigo 1:ad1952c5a3d7 548 }
stevenjigo 1:ad1952c5a3d7 549 if(PscPID==2){
stevenjigo 1:ad1952c5a3d7 550 ValPID=ValPID-0.1;
stevenjigo 1:ad1952c5a3d7 551 }
stevenjigo 1:ad1952c5a3d7 552 if(PscPID==3){
stevenjigo 1:ad1952c5a3d7 553 ValPID=ValPID-0.01;
stevenjigo 1:ad1952c5a3d7 554 }
stevenjigo 1:ad1952c5a3d7 555 ValPID=LimVarF(ValPID,CstPIDMin,CstPIDMax);
stevenjigo 1:ad1952c5a3d7 556 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 557 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 558 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 559 LCD.printf("%.2f",ValPID);
stevenjigo 1:ad1952c5a3d7 560 LCD.locate(PscPID-1,1);
stevenjigo 1:ad1952c5a3d7 561 while(!Bab);
stevenjigo 1:ad1952c5a3d7 562 }
stevenjigo 1:ad1952c5a3d7 563
stevenjigo 1:ad1952c5a3d7 564 goto ValPIDOpc;
stevenjigo 1:ad1952c5a3d7 565
stevenjigo 1:ad1952c5a3d7 566 Sensado:
stevenjigo 1:ad1952c5a3d7 567
stevenjigo 1:ad1952c5a3d7 568 if(!Bar){
stevenjigo 1:ad1952c5a3d7 569 ModoS--;
stevenjigo 1:ad1952c5a3d7 570 ModoS=LimVarI(ModoS,1,2);
stevenjigo 1:ad1952c5a3d7 571 while(!Bar);
stevenjigo 1:ad1952c5a3d7 572 goto Sensadoc;
stevenjigo 1:ad1952c5a3d7 573 }
stevenjigo 1:ad1952c5a3d7 574
stevenjigo 1:ad1952c5a3d7 575 if(!Bab){
stevenjigo 1:ad1952c5a3d7 576 ModoS++;
stevenjigo 1:ad1952c5a3d7 577 ModoS=LimVarI(ModoS,1,2);
stevenjigo 1:ad1952c5a3d7 578 while(!Bab);
stevenjigo 1:ad1952c5a3d7 579 goto Sensadoc;
stevenjigo 1:ad1952c5a3d7 580 }
stevenjigo 1:ad1952c5a3d7 581
stevenjigo 1:ad1952c5a3d7 582 if(!Bce){
stevenjigo 1:ad1952c5a3d7 583 LCD.cls();
stevenjigo 1:ad1952c5a3d7 584 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 585 LCD.printf("Modificar:");
stevenjigo 1:ad1952c5a3d7 586 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 587 LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 588 Modo=1;
stevenjigo 1:ad1952c5a3d7 589 while(!Bce);
stevenjigo 1:ad1952c5a3d7 590 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 591 }
stevenjigo 1:ad1952c5a3d7 592
stevenjigo 1:ad1952c5a3d7 593 goto Sensado;
stevenjigo 1:ad1952c5a3d7 594
stevenjigo 1:ad1952c5a3d7 595 //Estas son las 2 opciones para seleccionar el ModoS de operación, solo se ejecutarán luego de pulsar Bar o Bab.
stevenjigo 1:ad1952c5a3d7 596 //Se hace para evitar copiar dentro de cada if el mismo código.
stevenjigo 1:ad1952c5a3d7 597
stevenjigo 1:ad1952c5a3d7 598 Sensadoc:
stevenjigo 1:ad1952c5a3d7 599 if(ModoS==1){
stevenjigo 1:ad1952c5a3d7 600 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 601 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 602 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 603 LCD.printf("1. SRF05");
stevenjigo 1:ad1952c5a3d7 604 }
stevenjigo 1:ad1952c5a3d7 605 if(ModoS==2){
stevenjigo 1:ad1952c5a3d7 606 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 607 LCD.printf(" ");
stevenjigo 1:ad1952c5a3d7 608 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 609 LCD.printf("2. MPU6050");
stevenjigo 1:ad1952c5a3d7 610 }
stevenjigo 1:ad1952c5a3d7 611 goto Sensado;
stevenjigo 1:ad1952c5a3d7 612
stevenjigo 1:ad1952c5a3d7 613 OperacionM2:
stevenjigo 1:ad1952c5a3d7 614
stevenjigo 1:ad1952c5a3d7 615 Alt=US.read();
stevenjigo 1:ad1952c5a3d7 616 PC.printf(" Altura: %.2f ",Alt);
stevenjigo 1:ad1952c5a3d7 617 PC.printf(" Duty: %.2f \n",AcPID);
stevenjigo 1:ad1952c5a3d7 618 LCD.cls();
stevenjigo 1:ad1952c5a3d7 619 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 620 LCD.printf("%.2f",Alt);
stevenjigo 1:ad1952c5a3d7 621 Error=SP-Alt;
stevenjigo 1:ad1952c5a3d7 622 LCD.locate(8,0);
stevenjigo 1:ad1952c5a3d7 623 LCD.printf("%.2f",Error);
stevenjigo 1:ad1952c5a3d7 624 DeltaP=Kp*Error;
stevenjigo 1:ad1952c5a3d7 625 DeltaI=DeltaI+Ki*Error;
stevenjigo 1:ad1952c5a3d7 626 DeltaD=Kd*(Error-ErrorAnt);
stevenjigo 1:ad1952c5a3d7 627 AcPID=(DeltaP+DeltaI+DeltaD)*0.05+DtyMaxStd;
stevenjigo 1:ad1952c5a3d7 628 ErrorAnt=Error;
stevenjigo 1:ad1952c5a3d7 629 LCD.locate(8,1);
stevenjigo 1:ad1952c5a3d7 630 LCD.printf("%.2f",AcPID);
stevenjigo 1:ad1952c5a3d7 631 AcPID=SaturF(AcPID,DtyMin,9);
stevenjigo 1:ad1952c5a3d7 632 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 633 LCD.printf("%.2f",AcPID);
stevenjigo 1:ad1952c5a3d7 634 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 635 LCD.printf("%.2f",DtyMin);
stevenjigo 1:ad1952c5a3d7 636 PWM.write(AcPID*0.01);
stevenjigo 1:ad1952c5a3d7 637 wait_ms(DelayPID);
stevenjigo 1:ad1952c5a3d7 638
stevenjigo 1:ad1952c5a3d7 639 if(!Bre){
stevenjigo 1:ad1952c5a3d7 640 LCD.cls();
stevenjigo 1:ad1952c5a3d7 641 LCD.locate(0,0);
stevenjigo 1:ad1952c5a3d7 642 LCD.printf("Modificar:");
stevenjigo 1:ad1952c5a3d7 643 LCD.locate(0,1);
stevenjigo 1:ad1952c5a3d7 644 LCD.printf("1. Set Point");
stevenjigo 1:ad1952c5a3d7 645 Modo=1;
stevenjigo 1:ad1952c5a3d7 646 PWM.write(0.05);
stevenjigo 1:ad1952c5a3d7 647 while(!Bre);
stevenjigo 1:ad1952c5a3d7 648 goto MenuPID;
stevenjigo 1:ad1952c5a3d7 649 }
stevenjigo 1:ad1952c5a3d7 650
stevenjigo 1:ad1952c5a3d7 651 goto OperacionM2;
stevenjigo 1:ad1952c5a3d7 652
stevenjigo 1:ad1952c5a3d7 653 } // Corchete del While
stevenjigo 1:ad1952c5a3d7 654 } // Corchete del main