This program makes a PID control and the values por control action are entered by matrix keypad 4x4 interface.
Dependencies: FPointer TextLCD keypad mbed
main.cpp@0:54e8dab91b77, 2013-12-15 (annotated)
- Committer:
- lcorralesc1
- Date:
- Sun Dec 15 15:40:22 2013 +0000
- Revision:
- 0:54e8dab91b77
This program makes a PID control and the values por control action are entered by matrix keypad 4x4 interface.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lcorralesc1 | 0:54e8dab91b77 | 1 | //Programa para hacer control PID simple, ingresa parámetros con teclado 4x4. Imprime resultados en LCD 16x2 |
lcorralesc1 | 0:54e8dab91b77 | 2 | |
lcorralesc1 | 0:54e8dab91b77 | 3 | #include "mbed.h" |
lcorralesc1 | 0:54e8dab91b77 | 4 | #include "TextLCD.h" |
lcorralesc1 | 0:54e8dab91b77 | 5 | #include"keypad.h" //Librería del teclado 4x4 |
lcorralesc1 | 0:54e8dab91b77 | 6 | #include"FPointer.h" //Librería complementaria para libreria keypad |
lcorralesc1 | 0:54e8dab91b77 | 7 | |
lcorralesc1 | 0:54e8dab91b77 | 8 | TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 |
lcorralesc1 | 0:54e8dab91b77 | 9 | Keypad keypad(PTA2,PTD4,PTD3,PTD7,PTA13,PTD5,PTD0,PTD2); //Entradas del teclado |
lcorralesc1 | 0:54e8dab91b77 | 10 | |
lcorralesc1 | 0:54e8dab91b77 | 11 | AnalogIn Vin(PTB0); //Voltaje de alimentación |
lcorralesc1 | 0:54e8dab91b77 | 12 | AnalogOut Vout(PTE30); //Marcar la salida analógica |
lcorralesc1 | 0:54e8dab91b77 | 13 | |
lcorralesc1 | 0:54e8dab91b77 | 14 | //ASIGNACION DE VARIABLES |
lcorralesc1 | 0:54e8dab91b77 | 15 | Timer t; |
lcorralesc1 | 0:54e8dab91b77 | 16 | int cero; |
lcorralesc1 | 0:54e8dab91b77 | 17 | int flag; |
lcorralesc1 | 0:54e8dab91b77 | 18 | int q; |
lcorralesc1 | 0:54e8dab91b77 | 19 | int k; |
lcorralesc1 | 0:54e8dab91b77 | 20 | int C1=0x0E; //configurar el lcd para mostrar el guin bajo |
lcorralesc1 | 0:54e8dab91b77 | 21 | int C2=0x0C; //configurar el lcd para QUITAR el guion bajo |
lcorralesc1 | 0:54e8dab91b77 | 22 | float sp=0; //set point |
lcorralesc1 | 0:54e8dab91b77 | 23 | float kp=0; //ganancia proporcional |
lcorralesc1 | 0:54e8dab91b77 | 24 | float ki=0; //ganancia integral |
lcorralesc1 | 0:54e8dab91b77 | 25 | float kd=0; //ganancia derivativa |
lcorralesc1 | 0:54e8dab91b77 | 26 | int ind=0; //vector de caracteres. |
lcorralesc1 | 0:54e8dab91b77 | 27 | float err, med, yr, ap, ai, ad, err_v, cycle; //Variable de control PID |
lcorralesc1 | 0:54e8dab91b77 | 28 | float pid; |
lcorralesc1 | 0:54e8dab91b77 | 29 | |
lcorralesc1 | 0:54e8dab91b77 | 30 | //MATRIZ DEL TECLADO |
lcorralesc1 | 0:54e8dab91b77 | 31 | float Keytable[] = {1,2,3,11, |
lcorralesc1 | 0:54e8dab91b77 | 32 | 4,5,6,12, |
lcorralesc1 | 0:54e8dab91b77 | 33 | 7,8,9,13, |
lcorralesc1 | 0:54e8dab91b77 | 34 | 0,0,0,0}; |
lcorralesc1 | 0:54e8dab91b77 | 35 | |
lcorralesc1 | 0:54e8dab91b77 | 36 | //Se crea una función que incremente los valores |
lcorralesc1 | 0:54e8dab91b77 | 37 | void increment(int j){ |
lcorralesc1 | 0:54e8dab91b77 | 38 | if(j==0){ |
lcorralesc1 | 0:54e8dab91b77 | 39 | if (q!=1 && sp<10){ |
lcorralesc1 | 0:54e8dab91b77 | 40 | sp=ind; |
lcorralesc1 | 0:54e8dab91b77 | 41 | q=1; |
lcorralesc1 | 0:54e8dab91b77 | 42 | } |
lcorralesc1 | 0:54e8dab91b77 | 43 | else{ |
lcorralesc1 | 0:54e8dab91b77 | 44 | sp=10*sp+ind; |
lcorralesc1 | 0:54e8dab91b77 | 45 | } |
lcorralesc1 | 0:54e8dab91b77 | 46 | if(sp>999)sp=999; |
lcorralesc1 | 0:54e8dab91b77 | 47 | lcd.locate(3,0); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 48 | lcd.locate(3,0); lcd.printf("%.0f",sp); |
lcorralesc1 | 0:54e8dab91b77 | 49 | |
lcorralesc1 | 0:54e8dab91b77 | 50 | } |
lcorralesc1 | 0:54e8dab91b77 | 51 | else if(j==1){ |
lcorralesc1 | 0:54e8dab91b77 | 52 | if (q!=1 && kp<10){ |
lcorralesc1 | 0:54e8dab91b77 | 53 | kp=ind; |
lcorralesc1 | 0:54e8dab91b77 | 54 | q=1; |
lcorralesc1 | 0:54e8dab91b77 | 55 | } |
lcorralesc1 | 0:54e8dab91b77 | 56 | else{ |
lcorralesc1 | 0:54e8dab91b77 | 57 | kp=10*kp+ind; |
lcorralesc1 | 0:54e8dab91b77 | 58 | } |
lcorralesc1 | 0:54e8dab91b77 | 59 | if(kp>999)kp=999; |
lcorralesc1 | 0:54e8dab91b77 | 60 | lcd.locate(11,0); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 61 | lcd.locate(11,0); lcd.printf("%.0f",kp); |
lcorralesc1 | 0:54e8dab91b77 | 62 | } |
lcorralesc1 | 0:54e8dab91b77 | 63 | else if(j==2){ |
lcorralesc1 | 0:54e8dab91b77 | 64 | if (q!=1 && ki<10){ |
lcorralesc1 | 0:54e8dab91b77 | 65 | ki=ind; |
lcorralesc1 | 0:54e8dab91b77 | 66 | q=1; |
lcorralesc1 | 0:54e8dab91b77 | 67 | } |
lcorralesc1 | 0:54e8dab91b77 | 68 | else{ |
lcorralesc1 | 0:54e8dab91b77 | 69 | ki=10*ki+ind; |
lcorralesc1 | 0:54e8dab91b77 | 70 | } |
lcorralesc1 | 0:54e8dab91b77 | 71 | if(ki>999)ki=999; |
lcorralesc1 | 0:54e8dab91b77 | 72 | lcd.locate(3,1); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 73 | lcd.locate(3,1); lcd.printf("%.0f",ki); |
lcorralesc1 | 0:54e8dab91b77 | 74 | } |
lcorralesc1 | 0:54e8dab91b77 | 75 | else{ |
lcorralesc1 | 0:54e8dab91b77 | 76 | if (q!=1 && kd<10){ |
lcorralesc1 | 0:54e8dab91b77 | 77 | kd=ind; |
lcorralesc1 | 0:54e8dab91b77 | 78 | q=1; |
lcorralesc1 | 0:54e8dab91b77 | 79 | } |
lcorralesc1 | 0:54e8dab91b77 | 80 | else{ |
lcorralesc1 | 0:54e8dab91b77 | 81 | kd=10*kd+ind; |
lcorralesc1 | 0:54e8dab91b77 | 82 | } |
lcorralesc1 | 0:54e8dab91b77 | 83 | if(kd>999)kd=999; |
lcorralesc1 | 0:54e8dab91b77 | 84 | lcd.locate(11,1); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 85 | lcd.locate(11,1); lcd.printf("%.0f",kd); |
lcorralesc1 | 0:54e8dab91b77 | 86 | } |
lcorralesc1 | 0:54e8dab91b77 | 87 | ind=0; cero=0; |
lcorralesc1 | 0:54e8dab91b77 | 88 | } |
lcorralesc1 | 0:54e8dab91b77 | 89 | |
lcorralesc1 | 0:54e8dab91b77 | 90 | uint32_t cbAfterInput(uint32_t index) { |
lcorralesc1 | 0:54e8dab91b77 | 91 | ind=Keytable[index]; |
lcorralesc1 | 0:54e8dab91b77 | 92 | cero=index; |
lcorralesc1 | 0:54e8dab91b77 | 93 | return 0; |
lcorralesc1 | 0:54e8dab91b77 | 94 | } |
lcorralesc1 | 0:54e8dab91b77 | 95 | |
lcorralesc1 | 0:54e8dab91b77 | 96 | void def_posicion(int j){ |
lcorralesc1 | 0:54e8dab91b77 | 97 | if (j==0){ |
lcorralesc1 | 0:54e8dab91b77 | 98 | lcd.locate(3,0); lcd.printf("%.0f",sp); |
lcorralesc1 | 0:54e8dab91b77 | 99 | lcd.locate(3,0); |
lcorralesc1 | 0:54e8dab91b77 | 100 | } |
lcorralesc1 | 0:54e8dab91b77 | 101 | else if(j==1){ |
lcorralesc1 | 0:54e8dab91b77 | 102 | lcd.locate(11,0); lcd.printf("%.0f",kp); |
lcorralesc1 | 0:54e8dab91b77 | 103 | lcd.locate(11,0); |
lcorralesc1 | 0:54e8dab91b77 | 104 | } |
lcorralesc1 | 0:54e8dab91b77 | 105 | else if (j==2){ |
lcorralesc1 | 0:54e8dab91b77 | 106 | lcd.locate(3,1); lcd.printf("%.0f",ki); |
lcorralesc1 | 0:54e8dab91b77 | 107 | lcd.locate(3,1); |
lcorralesc1 | 0:54e8dab91b77 | 108 | } |
lcorralesc1 | 0:54e8dab91b77 | 109 | else { |
lcorralesc1 | 0:54e8dab91b77 | 110 | lcd.locate(11,1); lcd.printf("%.0f",kd); |
lcorralesc1 | 0:54e8dab91b77 | 111 | lcd.locate(11,1); |
lcorralesc1 | 0:54e8dab91b77 | 112 | } |
lcorralesc1 | 0:54e8dab91b77 | 113 | } |
lcorralesc1 | 0:54e8dab91b77 | 114 | |
lcorralesc1 | 0:54e8dab91b77 | 115 | //Dado que hay parámetros que no varía en el display en esta parte del código, se crea una función que los mantenga. |
lcorralesc1 | 0:54e8dab91b77 | 116 | void star_patch1(void){ |
lcorralesc1 | 0:54e8dab91b77 | 117 | lcd.cls(); |
lcorralesc1 | 0:54e8dab91b77 | 118 | lcd.locate(8,0); |
lcorralesc1 | 0:54e8dab91b77 | 119 | lcd.printf("Kp=%.0f",kp); |
lcorralesc1 | 0:54e8dab91b77 | 120 | lcd.locate(0,1); |
lcorralesc1 | 0:54e8dab91b77 | 121 | lcd.printf("Ki=%.0f",ki); |
lcorralesc1 | 0:54e8dab91b77 | 122 | lcd.locate(8,1); |
lcorralesc1 | 0:54e8dab91b77 | 123 | lcd.printf("Kd=%.0f",kd); |
lcorralesc1 | 0:54e8dab91b77 | 124 | lcd.writeCommand(C1); //cursor se vea y sea intermitente |
lcorralesc1 | 0:54e8dab91b77 | 125 | lcd.locate(0,0); |
lcorralesc1 | 0:54e8dab91b77 | 126 | lcd.printf("Sp=%.0f",sp); |
lcorralesc1 | 0:54e8dab91b77 | 127 | } |
lcorralesc1 | 0:54e8dab91b77 | 128 | |
lcorralesc1 | 0:54e8dab91b77 | 129 | void star_patch2(void){ // uso nuevamente función que imprime los caracteres que no van a variar en el display |
lcorralesc1 | 0:54e8dab91b77 | 130 | lcd.writeCommand(C2); |
lcorralesc1 | 0:54e8dab91b77 | 131 | lcd.cls(); |
lcorralesc1 | 0:54e8dab91b77 | 132 | lcd.printf("Iniciamos el PID |m|"); |
lcorralesc1 | 0:54e8dab91b77 | 133 | wait(2); |
lcorralesc1 | 0:54e8dab91b77 | 134 | lcd.cls(); |
lcorralesc1 | 0:54e8dab91b77 | 135 | lcd.printf("Er%=f",err); |
lcorralesc1 | 0:54e8dab91b77 | 136 | lcd.locate(8,0); lcd.printf("Me=%.0f",med); |
lcorralesc1 | 0:54e8dab91b77 | 137 | lcd.locate(0,1); lcd.printf("Sp=%.0f",sp); |
lcorralesc1 | 0:54e8dab91b77 | 138 | lcd.locate(8,1); lcd.printf("Co=%.0f",pid); |
lcorralesc1 | 0:54e8dab91b77 | 139 | wait(3); |
lcorralesc1 | 0:54e8dab91b77 | 140 | } |
lcorralesc1 | 0:54e8dab91b77 | 141 | |
lcorralesc1 | 0:54e8dab91b77 | 142 | int main(){ |
lcorralesc1 | 0:54e8dab91b77 | 143 | ini: |
lcorralesc1 | 0:54e8dab91b77 | 144 | ind=0; |
lcorralesc1 | 0:54e8dab91b77 | 145 | star_patch1(); |
lcorralesc1 | 0:54e8dab91b77 | 146 | keypad.CallAfterInput(&cbAfterInput); |
lcorralesc1 | 0:54e8dab91b77 | 147 | keypad.Start(); |
lcorralesc1 | 0:54e8dab91b77 | 148 | ini1: |
lcorralesc1 | 0:54e8dab91b77 | 149 | if(ind==12){ |
lcorralesc1 | 0:54e8dab91b77 | 150 | if (k<3) k++; |
lcorralesc1 | 0:54e8dab91b77 | 151 | else k=0; |
lcorralesc1 | 0:54e8dab91b77 | 152 | def_posicion(k); |
lcorralesc1 | 0:54e8dab91b77 | 153 | ind=0; |
lcorralesc1 | 0:54e8dab91b77 | 154 | q=0; |
lcorralesc1 | 0:54e8dab91b77 | 155 | } |
lcorralesc1 | 0:54e8dab91b77 | 156 | if(ind==13){ |
lcorralesc1 | 0:54e8dab91b77 | 157 | ind=0; |
lcorralesc1 | 0:54e8dab91b77 | 158 | goto PID; |
lcorralesc1 | 0:54e8dab91b77 | 159 | } |
lcorralesc1 | 0:54e8dab91b77 | 160 | if(ind==11){ |
lcorralesc1 | 0:54e8dab91b77 | 161 | if (k==0){ |
lcorralesc1 | 0:54e8dab91b77 | 162 | sp=0; |
lcorralesc1 | 0:54e8dab91b77 | 163 | lcd.locate(3,0); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 164 | lcd.locate(3,0); lcd.printf("%.0f",sp); |
lcorralesc1 | 0:54e8dab91b77 | 165 | } |
lcorralesc1 | 0:54e8dab91b77 | 166 | else if(k==1){ |
lcorralesc1 | 0:54e8dab91b77 | 167 | kp=0; |
lcorralesc1 | 0:54e8dab91b77 | 168 | lcd.locate(11,0); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 169 | lcd.locate(11,0); lcd.printf("%.0f",kp); |
lcorralesc1 | 0:54e8dab91b77 | 170 | } |
lcorralesc1 | 0:54e8dab91b77 | 171 | else if (k==2){ |
lcorralesc1 | 0:54e8dab91b77 | 172 | ki=0; |
lcorralesc1 | 0:54e8dab91b77 | 173 | lcd.locate(3,1); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 174 | lcd.locate(3,1); lcd.printf("%.0f",ki); |
lcorralesc1 | 0:54e8dab91b77 | 175 | } |
lcorralesc1 | 0:54e8dab91b77 | 176 | else { |
lcorralesc1 | 0:54e8dab91b77 | 177 | kd=0; |
lcorralesc1 | 0:54e8dab91b77 | 178 | lcd.locate(11,1); lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 179 | lcd.locate(11,1); lcd.printf("%.0f",kd); |
lcorralesc1 | 0:54e8dab91b77 | 180 | } |
lcorralesc1 | 0:54e8dab91b77 | 181 | q=0; ind=0; |
lcorralesc1 | 0:54e8dab91b77 | 182 | } |
lcorralesc1 | 0:54e8dab91b77 | 183 | if (ind!=0 && ind!=12 && ind!=13 && ind!=11 || cero==13 ){ |
lcorralesc1 | 0:54e8dab91b77 | 184 | increment(k); |
lcorralesc1 | 0:54e8dab91b77 | 185 | } |
lcorralesc1 | 0:54e8dab91b77 | 186 | goto ini1; |
lcorralesc1 | 0:54e8dab91b77 | 187 | |
lcorralesc1 | 0:54e8dab91b77 | 188 | PID: |
lcorralesc1 | 0:54e8dab91b77 | 189 | star_patch2(); |
lcorralesc1 | 0:54e8dab91b77 | 190 | |
lcorralesc1 | 0:54e8dab91b77 | 191 | while (1){ |
lcorralesc1 | 0:54e8dab91b77 | 192 | |
lcorralesc1 | 0:54e8dab91b77 | 193 | med=Vin.read()*1000; |
lcorralesc1 | 0:54e8dab91b77 | 194 | err =sp-med; |
lcorralesc1 | 0:54e8dab91b77 | 195 | ap = kp*err; |
lcorralesc1 | 0:54e8dab91b77 | 196 | ai =(ki*0.001*err)+ai; //calculo de la integral del error |
lcorralesc1 | 0:54e8dab91b77 | 197 | // VERIFICAMOS QUE LA ACCION INTEGRAL NO SEA MUY GRANDE!!! |
lcorralesc1 | 0:54e8dab91b77 | 198 | if(ai>0.5){ |
lcorralesc1 | 0:54e8dab91b77 | 199 | ai=0.5; |
lcorralesc1 | 0:54e8dab91b77 | 200 | } |
lcorralesc1 | 0:54e8dab91b77 | 201 | ad = kd*(err-err_v); //calculo de la accion derivativa |
lcorralesc1 | 0:54e8dab91b77 | 202 | pid =(ap+ai+ad); |
lcorralesc1 | 0:54e8dab91b77 | 203 | |
lcorralesc1 | 0:54e8dab91b77 | 204 | //MOSTRAMOS LAS VARIABLES |
lcorralesc1 | 0:54e8dab91b77 | 205 | if (pid > 999)pid=1000; |
lcorralesc1 | 0:54e8dab91b77 | 206 | if (pid<0)pid=0; |
lcorralesc1 | 0:54e8dab91b77 | 207 | |
lcorralesc1 | 0:54e8dab91b77 | 208 | |
lcorralesc1 | 0:54e8dab91b77 | 209 | Vout.write(pid/1000); |
lcorralesc1 | 0:54e8dab91b77 | 210 | if(flag==0){ |
lcorralesc1 | 0:54e8dab91b77 | 211 | t.start(); |
lcorralesc1 | 0:54e8dab91b77 | 212 | flag=1; |
lcorralesc1 | 0:54e8dab91b77 | 213 | } |
lcorralesc1 | 0:54e8dab91b77 | 214 | if (t>0.3){ |
lcorralesc1 | 0:54e8dab91b77 | 215 | lcd.locate(3,0); |
lcorralesc1 | 0:54e8dab91b77 | 216 | lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 217 | lcd.locate(3,0); |
lcorralesc1 | 0:54e8dab91b77 | 218 | lcd.printf("%.0f",err); |
lcorralesc1 | 0:54e8dab91b77 | 219 | lcd.locate(11,0); |
lcorralesc1 | 0:54e8dab91b77 | 220 | lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 221 | lcd.locate(11,0); |
lcorralesc1 | 0:54e8dab91b77 | 222 | lcd.printf("%.0f",med); |
lcorralesc1 | 0:54e8dab91b77 | 223 | lcd.locate(3,1); |
lcorralesc1 | 0:54e8dab91b77 | 224 | lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 225 | lcd.locate(3,1); |
lcorralesc1 | 0:54e8dab91b77 | 226 | lcd.printf("%.0f",sp); |
lcorralesc1 | 0:54e8dab91b77 | 227 | lcd.locate(11,1); |
lcorralesc1 | 0:54e8dab91b77 | 228 | lcd.printf(" "); |
lcorralesc1 | 0:54e8dab91b77 | 229 | lcd.locate(11,1); |
lcorralesc1 | 0:54e8dab91b77 | 230 | lcd.printf("%.2f ",pid/1000); |
lcorralesc1 | 0:54e8dab91b77 | 231 | t.reset(); |
lcorralesc1 | 0:54e8dab91b77 | 232 | flag=0; |
lcorralesc1 | 0:54e8dab91b77 | 233 | wait(.3); |
lcorralesc1 | 0:54e8dab91b77 | 234 | } |
lcorralesc1 | 0:54e8dab91b77 | 235 | |
lcorralesc1 | 0:54e8dab91b77 | 236 | //ACTUALIZACION DE LAS VARIABLES |
lcorralesc1 | 0:54e8dab91b77 | 237 | err_v = err; |
lcorralesc1 | 0:54e8dab91b77 | 238 | if(ind==13)goto ini; |
lcorralesc1 | 0:54e8dab91b77 | 239 | } |
lcorralesc1 | 0:54e8dab91b77 | 240 | } |