deiwid ricaurte
controlpid por control remoto
Dependencies: Pulse1 QEI TextLCD mbed
Fork of
irda1
by 
Gustavo Ramirez
main.cpp@3:786400a8fd7c, 2016-11-24 (annotated)
- Committer:
- deiwidricaurte
- Date:
- Thu Nov 24 01:06:46 2016 +0000
- Revision:
- 3:786400a8fd7c
- Parent:
- 2:6a15ab0305c8
controlpid con controlremoto;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| tony63 | 0:74d57f8ae247 | 1 | #include "mbed.h" |
| tony63 | 0:74d57f8ae247 | 2 | #include <Pulse1.h> |
| deiwidricaurte | 3:786400a8fd7c | 3 | #include "QEI.h" |
| deiwidricaurte | 3:786400a8fd7c | 4 | #include "TextLCD.h" |
| tony63 | 0:74d57f8ae247 | 5 | |
| deiwidricaurte | 3:786400a8fd7c | 6 | |
| deiwidricaurte | 3:786400a8fd7c | 7 | TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 |
| deiwidricaurte | 3:786400a8fd7c | 8 | AnalogIn y(PTB2);//entrada analoga |
| deiwidricaurte | 3:786400a8fd7c | 9 | AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer |
| tony63 | 2:6a15ab0305c8 | 10 | PulseInOut irda(PTD5);// en este puerto se pone el sensor infrarrojo |
| tony63 | 0:74d57f8ae247 | 11 | Serial pc(USBTX, USBRX); |
| tony63 | 0:74d57f8ae247 | 12 | DigitalOut led(LED1); |
| tony63 | 2:6a15ab0305c8 | 13 | DigitalOut led2(LED2); |
| tony63 | 2:6a15ab0305c8 | 14 | int header =0; //tiempo de cabecera pulso abajo |
| tony63 | 2:6a15ab0305c8 | 15 | const int head_H = 9100; //+20% medida con osciloscopio en microsegundos |
| tony63 | 2:6a15ab0305c8 | 16 | const int head_L = 8500;//-20% medida con osciloscopio |
| tony63 | 2:6a15ab0305c8 | 17 | int i=0; |
| tony63 | 2:6a15ab0305c8 | 18 | const int T_alto=1680;//ponga su tiempo de la prueba |
| tony63 | 2:6a15ab0305c8 | 19 | const int T_bajo=526;//ponga su tiempo de la prueba |
| tony63 | 2:6a15ab0305c8 | 20 | const int num_bits = 32;//ponga su numero de bits |
| tony63 | 2:6a15ab0305c8 | 21 | int num[num_bits];//cadena para almacenar todos los tiempos que conforman los bits de datos |
| deiwidricaurte | 3:786400a8fd7c | 22 | int num1[num_bits]; |
| deiwidricaurte | 3:786400a8fd7c | 23 | int arriba[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,1,1,0,1,0,0,0,0,0,0,1,0,1,1,1}; |
| deiwidricaurte | 3:786400a8fd7c | 24 | int abajo[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,1,1,0,0,0,0,1,1,0,0,1,1,1}; |
| deiwidricaurte | 3:786400a8fd7c | 25 | int derecha[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,1,1,1,0,0,0,1,1,0,0,0,1,1}; |
| deiwidricaurte | 3:786400a8fd7c | 26 | int ok[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,1,0,1,0,1,0,0,0,0,1,0,1,0,1,1}; |
| tony63 | 2:6a15ab0305c8 | 27 | int dato; // tiempo de cada dato que se lee |
| deiwidricaurte | 3:786400a8fd7c | 28 | int a=0; |
| deiwidricaurte | 3:786400a8fd7c | 29 | int b=0; |
| deiwidricaurte | 3:786400a8fd7c | 30 | int c=0; |
| deiwidricaurte | 3:786400a8fd7c | 31 | int d=0; |
| deiwidricaurte | 3:786400a8fd7c | 32 | |
| deiwidricaurte | 3:786400a8fd7c | 33 | |
| deiwidricaurte | 3:786400a8fd7c | 34 | int C2=0x18; // desplaza izquierda |
| deiwidricaurte | 3:786400a8fd7c | 35 | int C3=0x1A; // desplaza derecha |
| deiwidricaurte | 3:786400a8fd7c | 36 | int C4=0x0C; // quito cursor bajo |
| deiwidricaurte | 3:786400a8fd7c | 37 | |
| deiwidricaurte | 3:786400a8fd7c | 38 | int C1=0x0F; |
| deiwidricaurte | 3:786400a8fd7c | 39 | int cambio=0, diferencia=0; |
| deiwidricaurte | 3:786400a8fd7c | 40 | float pid,o,ai,ad,ap,med,err; |
| deiwidricaurte | 3:786400a8fd7c | 41 | float err_v; |
| deiwidricaurte | 3:786400a8fd7c | 42 | int sp=0,ki=0,kp=0,kd=0,pos=1; |
| deiwidricaurte | 3:786400a8fd7c | 43 | |
| deiwidricaurte | 3:786400a8fd7c | 44 | |
| tony63 | 0:74d57f8ae247 | 45 | int main(){ |
| deiwidricaurte | 3:786400a8fd7c | 46 | lcd.locate(0,1); |
| deiwidricaurte | 3:786400a8fd7c | 47 | lcd.printf("**Control PID**"); |
| deiwidricaurte | 3:786400a8fd7c | 48 | wait(2); |
| deiwidricaurte | 3:786400a8fd7c | 49 | lcd.cls(); // Borrar Pantalla |
| deiwidricaurte | 3:786400a8fd7c | 50 | lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD |
| deiwidricaurte | 3:786400a8fd7c | 51 | |
| deiwidricaurte | 3:786400a8fd7c | 52 | lcd.locate(8,0); |
| deiwidricaurte | 3:786400a8fd7c | 53 | lcd.printf("Kp=%d",kp); |
| deiwidricaurte | 3:786400a8fd7c | 54 | lcd.locate(0,1); |
| deiwidricaurte | 3:786400a8fd7c | 55 | lcd.printf("Ki=%d",ki); |
| deiwidricaurte | 3:786400a8fd7c | 56 | lcd.locate(8,1); |
| deiwidricaurte | 3:786400a8fd7c | 57 | lcd.printf("Kd=%d",kd); |
| deiwidricaurte | 3:786400a8fd7c | 58 | lcd.locate(0,0); |
| deiwidricaurte | 3:786400a8fd7c | 59 | lcd.printf("Sp=%d",sp); |
| deiwidricaurte | 3:786400a8fd7c | 60 | |
| tony63 | 0:74d57f8ae247 | 61 | while(1){ |
| deiwidricaurte | 3:786400a8fd7c | 62 | ini1: header=0; |
| deiwidricaurte | 3:786400a8fd7c | 63 | a=0; |
| deiwidricaurte | 3:786400a8fd7c | 64 | b=0; |
| deiwidricaurte | 3:786400a8fd7c | 65 | c=0; |
| deiwidricaurte | 3:786400a8fd7c | 66 | d=0; |
| tony63 | 2:6a15ab0305c8 | 67 | led2=1; |
| tony63 | 0:74d57f8ae247 | 68 | led=1; |
| tony63 | 2:6a15ab0305c8 | 69 | header = irda.read_low_us(); //funcion para leer un pulso de caida o bajo |
| tony63 | 2:6a15ab0305c8 | 70 | if (header > head_L && header < head_H) goto seguir;//verificar que este en la tolerancia +-20% |
| tony63 | 2:6a15ab0305c8 | 71 | else goto ini1; |
| tony63 | 0:74d57f8ae247 | 72 | |
| deiwidricaurte | 3:786400a8fd7c | 73 | seguir: |
| tony63 | 2:6a15ab0305c8 | 74 | //leo los datos de la trama y se meten a un arreglo |
| tony63 | 2:6a15ab0305c8 | 75 | wait_us(5000); |
| tony63 | 2:6a15ab0305c8 | 76 | led2=0; |
| deiwidricaurte | 3:786400a8fd7c | 77 | for(i=0;i<(num_bits-1);++i) |
| deiwidricaurte | 3:786400a8fd7c | 78 | { // POR OSCILOSCOPIO se determina que llegan (num_bits),datos |
| deiwidricaurte | 3:786400a8fd7c | 79 | dato = irda.read_high_us(); //leer un bit de datos que es pulso arriba en este control |
| deiwidricaurte | 3:786400a8fd7c | 80 | num[i]=dato; |
| deiwidricaurte | 3:786400a8fd7c | 81 | wait_us(300); |
| deiwidricaurte | 3:786400a8fd7c | 82 | } |
| tony63 | 2:6a15ab0305c8 | 83 | wait(0.5); //espero un poquito antes de leer todo el arreglo y ponerlo en pantalla |
| tony63 | 2:6a15ab0305c8 | 84 | pc.printf(",%d",header); |
| deiwidricaurte | 3:786400a8fd7c | 85 | for(i=0;i<(num_bits-1);++i) |
| deiwidricaurte | 3:786400a8fd7c | 86 | { |
| deiwidricaurte | 3:786400a8fd7c | 87 | pc.printf(",%d",num[i]); |
| deiwidricaurte | 3:786400a8fd7c | 88 | } |
| tony63 | 2:6a15ab0305c8 | 89 | wait(0.1); //espero e imprimo en binario |
| tony63 | 2:6a15ab0305c8 | 90 | pc.printf("\n\n"); |
| deiwidricaurte | 3:786400a8fd7c | 91 | for(i=0;i<(num_bits-1);++i) |
| deiwidricaurte | 3:786400a8fd7c | 92 | { |
| deiwidricaurte | 3:786400a8fd7c | 93 | |
| deiwidricaurte | 3:786400a8fd7c | 94 | if(num[i] > ((T_alto+T_bajo)/2)) |
| deiwidricaurte | 3:786400a8fd7c | 95 | { |
| deiwidricaurte | 3:786400a8fd7c | 96 | pc.printf("1"); |
| deiwidricaurte | 3:786400a8fd7c | 97 | num1[i]=1; |
| deiwidricaurte | 3:786400a8fd7c | 98 | if(num1[i]==arriba[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 99 | a=a+1; |
| deiwidricaurte | 3:786400a8fd7c | 100 | |
| deiwidricaurte | 3:786400a8fd7c | 101 | } |
| deiwidricaurte | 3:786400a8fd7c | 102 | if(num1[i]==abajo[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 103 | b=b+1; |
| deiwidricaurte | 3:786400a8fd7c | 104 | |
| deiwidricaurte | 3:786400a8fd7c | 105 | } |
| deiwidricaurte | 3:786400a8fd7c | 106 | if(num1[i]==derecha[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 107 | c=c+1; |
| deiwidricaurte | 3:786400a8fd7c | 108 | |
| deiwidricaurte | 3:786400a8fd7c | 109 | } |
| deiwidricaurte | 3:786400a8fd7c | 110 | if(num1[i]==ok[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 111 | d=d+1; |
| deiwidricaurte | 3:786400a8fd7c | 112 | |
| deiwidricaurte | 3:786400a8fd7c | 113 | } |
| deiwidricaurte | 3:786400a8fd7c | 114 | } |
| deiwidricaurte | 3:786400a8fd7c | 115 | else |
| deiwidricaurte | 3:786400a8fd7c | 116 | { |
| deiwidricaurte | 3:786400a8fd7c | 117 | pc.printf("0"); |
| deiwidricaurte | 3:786400a8fd7c | 118 | num1[i]=0; |
| deiwidricaurte | 3:786400a8fd7c | 119 | if(num1[i]==arriba[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 120 | a=a+1; |
| deiwidricaurte | 3:786400a8fd7c | 121 | } |
| deiwidricaurte | 3:786400a8fd7c | 122 | if(num1[i]==abajo[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 123 | b=b+1; |
| deiwidricaurte | 3:786400a8fd7c | 124 | } |
| deiwidricaurte | 3:786400a8fd7c | 125 | if(num1[i]==derecha[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 126 | c=c+1; |
| deiwidricaurte | 3:786400a8fd7c | 127 | } |
| deiwidricaurte | 3:786400a8fd7c | 128 | if(num1[i]==ok[i]){ |
| deiwidricaurte | 3:786400a8fd7c | 129 | d=d+1; |
| deiwidricaurte | 3:786400a8fd7c | 130 | } |
| deiwidricaurte | 3:786400a8fd7c | 131 | } |
| deiwidricaurte | 3:786400a8fd7c | 132 | } |
| deiwidricaurte | 3:786400a8fd7c | 133 | if(a==31) |
| deiwidricaurte | 3:786400a8fd7c | 134 | { |
| deiwidricaurte | 3:786400a8fd7c | 135 | a=0; |
| deiwidricaurte | 3:786400a8fd7c | 136 | b=0; |
| deiwidricaurte | 3:786400a8fd7c | 137 | c=0; |
| deiwidricaurte | 3:786400a8fd7c | 138 | d=0; |
| deiwidricaurte | 3:786400a8fd7c | 139 | if(pos==1) |
| deiwidricaurte | 3:786400a8fd7c | 140 | { |
| deiwidricaurte | 3:786400a8fd7c | 141 | |
| deiwidricaurte | 3:786400a8fd7c | 142 | if(sp+1>=999) |
| deiwidricaurte | 3:786400a8fd7c | 143 | { |
| deiwidricaurte | 3:786400a8fd7c | 144 | sp=999; |
| deiwidricaurte | 3:786400a8fd7c | 145 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 146 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 147 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 148 | lcd.printf("%d", sp); |
| deiwidricaurte | 3:786400a8fd7c | 149 | } |
| deiwidricaurte | 3:786400a8fd7c | 150 | else |
| deiwidricaurte | 3:786400a8fd7c | 151 | { |
| deiwidricaurte | 3:786400a8fd7c | 152 | sp+=1; |
| deiwidricaurte | 3:786400a8fd7c | 153 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 154 | lcd.printf("%d", sp); |
| deiwidricaurte | 3:786400a8fd7c | 155 | |
| deiwidricaurte | 3:786400a8fd7c | 156 | } |
| deiwidricaurte | 3:786400a8fd7c | 157 | } |
| deiwidricaurte | 3:786400a8fd7c | 158 | else if(pos==2) |
| deiwidricaurte | 3:786400a8fd7c | 159 | { |
| deiwidricaurte | 3:786400a8fd7c | 160 | |
| deiwidricaurte | 3:786400a8fd7c | 161 | if(kp+1>=999) |
| deiwidricaurte | 3:786400a8fd7c | 162 | { |
| deiwidricaurte | 3:786400a8fd7c | 163 | kp=999; |
| deiwidricaurte | 3:786400a8fd7c | 164 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 165 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 166 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 167 | lcd.printf("%d", kp); |
| deiwidricaurte | 3:786400a8fd7c | 168 | } |
| deiwidricaurte | 3:786400a8fd7c | 169 | else |
| deiwidricaurte | 3:786400a8fd7c | 170 | { |
| deiwidricaurte | 3:786400a8fd7c | 171 | kp+=1; |
| deiwidricaurte | 3:786400a8fd7c | 172 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 173 | lcd.printf("%d", kp); |
| deiwidricaurte | 3:786400a8fd7c | 174 | |
| deiwidricaurte | 3:786400a8fd7c | 175 | } |
| deiwidricaurte | 3:786400a8fd7c | 176 | } |
| deiwidricaurte | 3:786400a8fd7c | 177 | else if(pos==3) |
| deiwidricaurte | 3:786400a8fd7c | 178 | { |
| deiwidricaurte | 3:786400a8fd7c | 179 | |
| deiwidricaurte | 3:786400a8fd7c | 180 | if(ki+1>=999) |
| deiwidricaurte | 3:786400a8fd7c | 181 | { |
| deiwidricaurte | 3:786400a8fd7c | 182 | sp=999; |
| deiwidricaurte | 3:786400a8fd7c | 183 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 184 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 185 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 186 | lcd.printf("%d", ki); |
| deiwidricaurte | 3:786400a8fd7c | 187 | } |
| deiwidricaurte | 3:786400a8fd7c | 188 | else |
| deiwidricaurte | 3:786400a8fd7c | 189 | { |
| deiwidricaurte | 3:786400a8fd7c | 190 | ki+=1; |
| deiwidricaurte | 3:786400a8fd7c | 191 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 192 | lcd.printf("%d", ki); |
| deiwidricaurte | 3:786400a8fd7c | 193 | |
| deiwidricaurte | 3:786400a8fd7c | 194 | } |
| deiwidricaurte | 3:786400a8fd7c | 195 | } |
| deiwidricaurte | 3:786400a8fd7c | 196 | else if(pos==4) |
| deiwidricaurte | 3:786400a8fd7c | 197 | { |
| deiwidricaurte | 3:786400a8fd7c | 198 | |
| deiwidricaurte | 3:786400a8fd7c | 199 | if(kd+1>=999) |
| deiwidricaurte | 3:786400a8fd7c | 200 | { |
| deiwidricaurte | 3:786400a8fd7c | 201 | sp=999; |
| deiwidricaurte | 3:786400a8fd7c | 202 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 203 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 204 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 205 | lcd.printf("%d", kd); |
| deiwidricaurte | 3:786400a8fd7c | 206 | } |
| deiwidricaurte | 3:786400a8fd7c | 207 | else |
| deiwidricaurte | 3:786400a8fd7c | 208 | { |
| deiwidricaurte | 3:786400a8fd7c | 209 | kd+=1; |
| deiwidricaurte | 3:786400a8fd7c | 210 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 211 | lcd.printf("%d", kd); |
| deiwidricaurte | 3:786400a8fd7c | 212 | |
| deiwidricaurte | 3:786400a8fd7c | 213 | } |
| deiwidricaurte | 3:786400a8fd7c | 214 | } |
| deiwidricaurte | 3:786400a8fd7c | 215 | } |
| deiwidricaurte | 3:786400a8fd7c | 216 | |
| deiwidricaurte | 3:786400a8fd7c | 217 | else if(b==31) |
| deiwidricaurte | 3:786400a8fd7c | 218 | { |
| deiwidricaurte | 3:786400a8fd7c | 219 | a=0; |
| deiwidricaurte | 3:786400a8fd7c | 220 | b=0; |
| deiwidricaurte | 3:786400a8fd7c | 221 | c=0; |
| deiwidricaurte | 3:786400a8fd7c | 222 | d=0; |
| deiwidricaurte | 3:786400a8fd7c | 223 | if(pos==1) |
| deiwidricaurte | 3:786400a8fd7c | 224 | { |
| deiwidricaurte | 3:786400a8fd7c | 225 | |
| deiwidricaurte | 3:786400a8fd7c | 226 | if(sp-1<=0) |
| deiwidricaurte | 3:786400a8fd7c | 227 | { |
| deiwidricaurte | 3:786400a8fd7c | 228 | sp=0; |
| deiwidricaurte | 3:786400a8fd7c | 229 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 230 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 231 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 232 | lcd.printf("%d", sp); |
| deiwidricaurte | 3:786400a8fd7c | 233 | } |
| deiwidricaurte | 3:786400a8fd7c | 234 | else |
| deiwidricaurte | 3:786400a8fd7c | 235 | { |
| deiwidricaurte | 3:786400a8fd7c | 236 | sp-=1; |
| deiwidricaurte | 3:786400a8fd7c | 237 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 238 | lcd.printf("%d", sp); |
| deiwidricaurte | 3:786400a8fd7c | 239 | |
| deiwidricaurte | 3:786400a8fd7c | 240 | } |
| deiwidricaurte | 3:786400a8fd7c | 241 | } |
| deiwidricaurte | 3:786400a8fd7c | 242 | else if(pos==2) |
| deiwidricaurte | 3:786400a8fd7c | 243 | { |
| deiwidricaurte | 3:786400a8fd7c | 244 | |
| deiwidricaurte | 3:786400a8fd7c | 245 | if(kp-1<=0) |
| deiwidricaurte | 3:786400a8fd7c | 246 | { |
| deiwidricaurte | 3:786400a8fd7c | 247 | kp=0; |
| deiwidricaurte | 3:786400a8fd7c | 248 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 249 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 250 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 251 | lcd.printf("%d", kp); |
| deiwidricaurte | 3:786400a8fd7c | 252 | } |
| deiwidricaurte | 3:786400a8fd7c | 253 | else |
| deiwidricaurte | 3:786400a8fd7c | 254 | { |
| deiwidricaurte | 3:786400a8fd7c | 255 | kp-=1; |
| deiwidricaurte | 3:786400a8fd7c | 256 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 257 | lcd.printf("%d", kp); |
| deiwidricaurte | 3:786400a8fd7c | 258 | |
| deiwidricaurte | 3:786400a8fd7c | 259 | } |
| deiwidricaurte | 3:786400a8fd7c | 260 | } |
| deiwidricaurte | 3:786400a8fd7c | 261 | else if(pos==3) |
| deiwidricaurte | 3:786400a8fd7c | 262 | { |
| deiwidricaurte | 3:786400a8fd7c | 263 | |
| deiwidricaurte | 3:786400a8fd7c | 264 | if(ki-1<=0) |
| deiwidricaurte | 3:786400a8fd7c | 265 | { |
| deiwidricaurte | 3:786400a8fd7c | 266 | sp=0; |
| deiwidricaurte | 3:786400a8fd7c | 267 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 268 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 269 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 270 | lcd.printf("%d", ki); |
| deiwidricaurte | 3:786400a8fd7c | 271 | } |
| deiwidricaurte | 3:786400a8fd7c | 272 | else |
| deiwidricaurte | 3:786400a8fd7c | 273 | { |
| deiwidricaurte | 3:786400a8fd7c | 274 | ki-=1; |
| deiwidricaurte | 3:786400a8fd7c | 275 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 276 | lcd.printf("%d", ki); |
| deiwidricaurte | 3:786400a8fd7c | 277 | |
| deiwidricaurte | 3:786400a8fd7c | 278 | } |
| deiwidricaurte | 3:786400a8fd7c | 279 | } |
| deiwidricaurte | 3:786400a8fd7c | 280 | else if(pos==4) |
| deiwidricaurte | 3:786400a8fd7c | 281 | { |
| deiwidricaurte | 3:786400a8fd7c | 282 | |
| deiwidricaurte | 3:786400a8fd7c | 283 | if(kd-1<=0) |
| deiwidricaurte | 3:786400a8fd7c | 284 | { |
| deiwidricaurte | 3:786400a8fd7c | 285 | sp=0; |
| deiwidricaurte | 3:786400a8fd7c | 286 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 287 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 288 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 289 | lcd.printf("%d", kd); |
| deiwidricaurte | 3:786400a8fd7c | 290 | } |
| deiwidricaurte | 3:786400a8fd7c | 291 | else |
| deiwidricaurte | 3:786400a8fd7c | 292 | { |
| deiwidricaurte | 3:786400a8fd7c | 293 | kd-=1; |
| deiwidricaurte | 3:786400a8fd7c | 294 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 295 | lcd.printf("%d", kd); |
| deiwidricaurte | 3:786400a8fd7c | 296 | |
| deiwidricaurte | 3:786400a8fd7c | 297 | } |
| deiwidricaurte | 3:786400a8fd7c | 298 | } |
| deiwidricaurte | 3:786400a8fd7c | 299 | } |
| deiwidricaurte | 3:786400a8fd7c | 300 | |
| deiwidricaurte | 3:786400a8fd7c | 301 | if(c==31) |
| deiwidricaurte | 3:786400a8fd7c | 302 | { |
| deiwidricaurte | 3:786400a8fd7c | 303 | a=0; |
| deiwidricaurte | 3:786400a8fd7c | 304 | b=0; |
| deiwidricaurte | 3:786400a8fd7c | 305 | c=0; |
| deiwidricaurte | 3:786400a8fd7c | 306 | d=0; |
| deiwidricaurte | 3:786400a8fd7c | 307 | led2 =!led2; |
| deiwidricaurte | 3:786400a8fd7c | 308 | if(pos==4) |
| deiwidricaurte | 3:786400a8fd7c | 309 | { |
| deiwidricaurte | 3:786400a8fd7c | 310 | pos=1; |
| deiwidricaurte | 3:786400a8fd7c | 311 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 312 | lcd.printf("%d", sp); |
| deiwidricaurte | 3:786400a8fd7c | 313 | } |
| deiwidricaurte | 3:786400a8fd7c | 314 | else if (pos==1) |
| deiwidricaurte | 3:786400a8fd7c | 315 | { |
| deiwidricaurte | 3:786400a8fd7c | 316 | pos++; |
| deiwidricaurte | 3:786400a8fd7c | 317 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 318 | lcd.printf("%d", kp); |
| deiwidricaurte | 3:786400a8fd7c | 319 | } |
| deiwidricaurte | 3:786400a8fd7c | 320 | else if(pos==2) |
| deiwidricaurte | 3:786400a8fd7c | 321 | { |
| deiwidricaurte | 3:786400a8fd7c | 322 | pos++; |
| deiwidricaurte | 3:786400a8fd7c | 323 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 324 | lcd.printf("%d", ki); |
| deiwidricaurte | 3:786400a8fd7c | 325 | } |
| deiwidricaurte | 3:786400a8fd7c | 326 | else if(pos==3) |
| deiwidricaurte | 3:786400a8fd7c | 327 | { |
| deiwidricaurte | 3:786400a8fd7c | 328 | pos++; |
| deiwidricaurte | 3:786400a8fd7c | 329 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 330 | lcd.printf("%d", kd); |
| deiwidricaurte | 3:786400a8fd7c | 331 | } |
| deiwidricaurte | 3:786400a8fd7c | 332 | wait(0.25); |
| deiwidricaurte | 3:786400a8fd7c | 333 | } |
| deiwidricaurte | 3:786400a8fd7c | 334 | |
| deiwidricaurte | 3:786400a8fd7c | 335 | if(d==31) |
| deiwidricaurte | 3:786400a8fd7c | 336 | { |
| deiwidricaurte | 3:786400a8fd7c | 337 | a=0; |
| deiwidricaurte | 3:786400a8fd7c | 338 | b=0; |
| deiwidricaurte | 3:786400a8fd7c | 339 | c=0; |
| deiwidricaurte | 3:786400a8fd7c | 340 | d=0; |
| deiwidricaurte | 3:786400a8fd7c | 341 | break; //sale del bucle si pulsan ok |
| deiwidricaurte | 3:786400a8fd7c | 342 | } |
| deiwidricaurte | 3:786400a8fd7c | 343 | else |
| deiwidricaurte | 3:786400a8fd7c | 344 | { |
| deiwidricaurte | 3:786400a8fd7c | 345 | a=0; |
| deiwidricaurte | 3:786400a8fd7c | 346 | b=0; |
| deiwidricaurte | 3:786400a8fd7c | 347 | c=0; |
| deiwidricaurte | 3:786400a8fd7c | 348 | d=0; |
| deiwidricaurte | 3:786400a8fd7c | 349 | } |
| deiwidricaurte | 3:786400a8fd7c | 350 | goto ini1; |
| deiwidricaurte | 3:786400a8fd7c | 351 | |
| deiwidricaurte | 3:786400a8fd7c | 352 | |
| deiwidricaurte | 3:786400a8fd7c | 353 | } |
| deiwidricaurte | 3:786400a8fd7c | 354 | //Transicion |
| deiwidricaurte | 3:786400a8fd7c | 355 | lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo |
| deiwidricaurte | 3:786400a8fd7c | 356 | lcd.cls(); //borra la pantalla |
| deiwidricaurte | 3:786400a8fd7c | 357 | lcd.printf(" GUARDADOS!"); |
| deiwidricaurte | 3:786400a8fd7c | 358 | wait(1); |
| deiwidricaurte | 3:786400a8fd7c | 359 | lcd.cls(); |
| deiwidricaurte | 3:786400a8fd7c | 360 | lcd.printf(" INICIA EL PID"); |
| deiwidricaurte | 3:786400a8fd7c | 361 | wait(1); |
| deiwidricaurte | 3:786400a8fd7c | 362 | // se imprimen los parches del control ***************************************** |
| deiwidricaurte | 3:786400a8fd7c | 363 | lcd.cls(); |
| deiwidricaurte | 3:786400a8fd7c | 364 | lcd.printf("Er=%3.0f",err); |
| deiwidricaurte | 3:786400a8fd7c | 365 | lcd.locate(8,0); |
| deiwidricaurte | 3:786400a8fd7c | 366 | lcd.printf("Me=%3.0f",med); |
| deiwidricaurte | 3:786400a8fd7c | 367 | lcd.locate(0,1); |
| deiwidricaurte | 3:786400a8fd7c | 368 | lcd.printf("Sp=%3.0f",sp); |
| deiwidricaurte | 3:786400a8fd7c | 369 | lcd.locate(8,1); |
| deiwidricaurte | 3:786400a8fd7c | 370 | lcd.printf("Co=%3.0f",pid); |
| deiwidricaurte | 3:786400a8fd7c | 371 | wait(1); |
| deiwidricaurte | 3:786400a8fd7c | 372 | |
| deiwidricaurte | 3:786400a8fd7c | 373 | // CICLO PRINCIPAL CONTROLADOR PID |
| deiwidricaurte | 3:786400a8fd7c | 374 | lop1: med = y.read()*999; |
| deiwidricaurte | 3:786400a8fd7c | 375 | err = (sp-med); //se calcula el error |
| deiwidricaurte | 3:786400a8fd7c | 376 | ap = kp*err*0.01f; //se calcula la accion proporcinal |
| deiwidricaurte | 3:786400a8fd7c | 377 | ai =(ki*err*0.01f)+ai; //calculo de la integral del error |
| deiwidricaurte | 3:786400a8fd7c | 378 | ad = kd*(err-err_v)*0.01f; //calculo de la accion derivativa |
| deiwidricaurte | 3:786400a8fd7c | 379 | pid = (ap+ai+ad); |
| deiwidricaurte | 3:786400a8fd7c | 380 | // se verifica que pid sea positivo ************************************** |
| deiwidricaurte | 3:786400a8fd7c | 381 | if(pid<=0) |
| deiwidricaurte | 3:786400a8fd7c | 382 | { |
| deiwidricaurte | 3:786400a8fd7c | 383 | pid=0; |
| deiwidricaurte | 3:786400a8fd7c | 384 | } |
| deiwidricaurte | 3:786400a8fd7c | 385 | |
| deiwidricaurte | 3:786400a8fd7c | 386 | // se verifica que pid sea menor o igual la valor maximo ***************** |
| deiwidricaurte | 3:786400a8fd7c | 387 | if (pid > 999) |
| deiwidricaurte | 3:786400a8fd7c | 388 | { |
| deiwidricaurte | 3:786400a8fd7c | 389 | pid=999; |
| deiwidricaurte | 3:786400a8fd7c | 390 | } |
| deiwidricaurte | 3:786400a8fd7c | 391 | |
| deiwidricaurte | 3:786400a8fd7c | 392 | |
| deiwidricaurte | 3:786400a8fd7c | 393 | //se muestran las variables****************************************** |
| deiwidricaurte | 3:786400a8fd7c | 394 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 395 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 396 | lcd.locate(3,0); |
| deiwidricaurte | 3:786400a8fd7c | 397 | lcd.printf("%3.0f",err); |
| deiwidricaurte | 3:786400a8fd7c | 398 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 399 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 400 | lcd.locate(11,0); |
| deiwidricaurte | 3:786400a8fd7c | 401 | lcd.printf("%3.0f",med); |
| deiwidricaurte | 3:786400a8fd7c | 402 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 403 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 404 | lcd.locate(3,1); |
| deiwidricaurte | 3:786400a8fd7c | 405 | lcd.printf("%d",sp); |
| deiwidricaurte | 3:786400a8fd7c | 406 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 407 | lcd.printf(" "); |
| deiwidricaurte | 3:786400a8fd7c | 408 | lcd.locate(11,1); |
| deiwidricaurte | 3:786400a8fd7c | 409 | lcd.printf("%3.0f",pid); |
| deiwidricaurte | 3:786400a8fd7c | 410 | |
| deiwidricaurte | 3:786400a8fd7c | 411 | |
| deiwidricaurte | 3:786400a8fd7c | 412 | |
| deiwidricaurte | 3:786400a8fd7c | 413 | |
| deiwidricaurte | 3:786400a8fd7c | 414 | //Normalizacion de la salida |
| deiwidricaurte | 3:786400a8fd7c | 415 | // se actualizan las variables ******************************************* |
| deiwidricaurte | 3:786400a8fd7c | 416 | err_v = err; |
| deiwidricaurte | 3:786400a8fd7c | 417 | |
| deiwidricaurte | 3:786400a8fd7c | 418 | o = pid/999; |
| deiwidricaurte | 3:786400a8fd7c | 419 | u.write(o); |
| deiwidricaurte | 3:786400a8fd7c | 420 | |
| deiwidricaurte | 3:786400a8fd7c | 421 | wait_ms(300); |
| deiwidricaurte | 3:786400a8fd7c | 422 | goto lop1; |
| deiwidricaurte | 3:786400a8fd7c | 423 | } |