![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
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?
User | Revision | Line number | New 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 |