![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Programa para colocar las constantes de un PID con encoder y controlar una planta
Dependencies: DebounceIn TextLCD mbed
main.cpp@0:65c8fa8a589b, 2015-10-23 (annotated)
- Committer:
- GermanD
- Date:
- Fri Oct 23 20:45:49 2015 +0000
- Revision:
- 0:65c8fa8a589b
Programa para colocar las constantes de un controlador PID con un encoder y hacer el control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GermanD | 0:65c8fa8a589b | 1 | #include "mbed.h" |
GermanD | 0:65c8fa8a589b | 2 | #include "DebouncedIn.h" |
GermanD | 0:65c8fa8a589b | 3 | #include "TextLCD.h" |
GermanD | 0:65c8fa8a589b | 4 | #include "QEI.h" |
GermanD | 0:65c8fa8a589b | 5 | |
GermanD | 0:65c8fa8a589b | 6 | AnalogIn Vin(PTC2); // entrada analoga |
GermanD | 0:65c8fa8a589b | 7 | AnalogOut Vout(PTE30); // salida analoga |
GermanD | 0:65c8fa8a589b | 8 | //voltaje de salida maximo= 3.3 V |
GermanD | 0:65c8fa8a589b | 9 | |
GermanD | 0:65c8fa8a589b | 10 | TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 |
GermanD | 0:65c8fa8a589b | 11 | |
GermanD | 0:65c8fa8a589b | 12 | QEI wheel (PTD5, PTD0, NC, 150); |
GermanD | 0:65c8fa8a589b | 13 | DigitalOut led1(LED1); |
GermanD | 0:65c8fa8a589b | 14 | DigitalOut led2(LED2); |
GermanD | 0:65c8fa8a589b | 15 | DigitalOut led3(LED3); |
GermanD | 0:65c8fa8a589b | 16 | DebouncedIn botonEncoder(PTC5); |
GermanD | 0:65c8fa8a589b | 17 | DebouncedIn button4(PTC17); // pulsador par dar "enter" |
GermanD | 0:65c8fa8a589b | 18 | |
GermanD | 0:65c8fa8a589b | 19 | |
GermanD | 0:65c8fa8a589b | 20 | //int C1=0x0E; // muestra el cursor |
GermanD | 0:65c8fa8a589b | 21 | int C2=0x18; // desplaza izquierda |
GermanD | 0:65c8fa8a589b | 22 | int C3=0x1A; // desplaza derecha |
GermanD | 0:65c8fa8a589b | 23 | int C4=0x0C; // quito cursor bajo |
GermanD | 0:65c8fa8a589b | 24 | int C1=0x0F; |
GermanD | 0:65c8fa8a589b | 25 | int sp=0,kp=0,kd=0,ki=0,p=1,bandera = 0; |
GermanD | 0:65c8fa8a589b | 26 | int i; // indice de la variable |
GermanD | 0:65c8fa8a589b | 27 | int j; //variable controla cambio 4 posiciones |
GermanD | 0:65c8fa8a589b | 28 | float err; |
GermanD | 0:65c8fa8a589b | 29 | float pwmset; |
GermanD | 0:65c8fa8a589b | 30 | float eInteg; |
GermanD | 0:65c8fa8a589b | 31 | float pGain; |
GermanD | 0:65c8fa8a589b | 32 | float ePrev ; |
GermanD | 0:65c8fa8a589b | 33 | float iGain; |
GermanD | 0:65c8fa8a589b | 34 | float dGain; |
GermanD | 0:65c8fa8a589b | 35 | float x; |
GermanD | 0:65c8fa8a589b | 36 | int main() |
GermanD | 0:65c8fa8a589b | 37 | { |
GermanD | 0:65c8fa8a589b | 38 | lcd.cls(); |
GermanD | 0:65c8fa8a589b | 39 | lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD |
GermanD | 0:65c8fa8a589b | 40 | lcd.locate(8,0); |
GermanD | 0:65c8fa8a589b | 41 | lcd.printf("kp=%d",kp); |
GermanD | 0:65c8fa8a589b | 42 | lcd.locate(0,1); |
GermanD | 0:65c8fa8a589b | 43 | lcd.printf("Ki=%d", ki); |
GermanD | 0:65c8fa8a589b | 44 | lcd.locate(8,1); |
GermanD | 0:65c8fa8a589b | 45 | lcd.printf("Kd=%d", kd); |
GermanD | 0:65c8fa8a589b | 46 | lcd.locate(0,0); |
GermanD | 0:65c8fa8a589b | 47 | lcd.printf("Sp=%d", sp); |
GermanD | 0:65c8fa8a589b | 48 | |
GermanD | 0:65c8fa8a589b | 49 | while(1) { |
GermanD | 0:65c8fa8a589b | 50 | |
GermanD | 0:65c8fa8a589b | 51 | led3 =1; |
GermanD | 0:65c8fa8a589b | 52 | if (botonEncoder.falling()) { //INCREMENTA POSICION DEL MENU CON EL SWITCH DEL ENCODER |
GermanD | 0:65c8fa8a589b | 53 | led3 =!led3; |
GermanD | 0:65c8fa8a589b | 54 | ++j; |
GermanD | 0:65c8fa8a589b | 55 | } |
GermanD | 0:65c8fa8a589b | 56 | |
GermanD | 0:65c8fa8a589b | 57 | if (j==0){ |
GermanD | 0:65c8fa8a589b | 58 | sp=sp+wheel.getPulses(); |
GermanD | 0:65c8fa8a589b | 59 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 60 | if (sp>5){ |
GermanD | 0:65c8fa8a589b | 61 | sp=5; |
GermanD | 0:65c8fa8a589b | 62 | } |
GermanD | 0:65c8fa8a589b | 63 | if (sp<0){ |
GermanD | 0:65c8fa8a589b | 64 | sp=0; |
GermanD | 0:65c8fa8a589b | 65 | } |
GermanD | 0:65c8fa8a589b | 66 | lcd.locate(3,0); |
GermanD | 0:65c8fa8a589b | 67 | lcd.printf(" ",sp); |
GermanD | 0:65c8fa8a589b | 68 | lcd.locate(3,0); |
GermanD | 0:65c8fa8a589b | 69 | lcd.printf("%d",sp); |
GermanD | 0:65c8fa8a589b | 70 | wait(0.2); |
GermanD | 0:65c8fa8a589b | 71 | |
GermanD | 0:65c8fa8a589b | 72 | if(botonEncoder.falling()){ |
GermanD | 0:65c8fa8a589b | 73 | j=1; |
GermanD | 0:65c8fa8a589b | 74 | led3=0; |
GermanD | 0:65c8fa8a589b | 75 | wait(0.3); |
GermanD | 0:65c8fa8a589b | 76 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 77 | } |
GermanD | 0:65c8fa8a589b | 78 | |
GermanD | 0:65c8fa8a589b | 79 | } |
GermanD | 0:65c8fa8a589b | 80 | |
GermanD | 0:65c8fa8a589b | 81 | if (j==1) { |
GermanD | 0:65c8fa8a589b | 82 | kp=kp+wheel.getPulses(); |
GermanD | 0:65c8fa8a589b | 83 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 84 | if (kp>999){ |
GermanD | 0:65c8fa8a589b | 85 | kp=999; |
GermanD | 0:65c8fa8a589b | 86 | } |
GermanD | 0:65c8fa8a589b | 87 | if (kp<0){ |
GermanD | 0:65c8fa8a589b | 88 | kp=0; |
GermanD | 0:65c8fa8a589b | 89 | } |
GermanD | 0:65c8fa8a589b | 90 | lcd.locate(11,0); |
GermanD | 0:65c8fa8a589b | 91 | lcd.printf(" "); |
GermanD | 0:65c8fa8a589b | 92 | lcd.locate(11,0); |
GermanD | 0:65c8fa8a589b | 93 | lcd.printf("%d",kp); |
GermanD | 0:65c8fa8a589b | 94 | wait(0.2); |
GermanD | 0:65c8fa8a589b | 95 | |
GermanD | 0:65c8fa8a589b | 96 | if(botonEncoder.falling()){ |
GermanD | 0:65c8fa8a589b | 97 | j=2; |
GermanD | 0:65c8fa8a589b | 98 | led3=0; |
GermanD | 0:65c8fa8a589b | 99 | wait(0.3); |
GermanD | 0:65c8fa8a589b | 100 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 101 | } |
GermanD | 0:65c8fa8a589b | 102 | |
GermanD | 0:65c8fa8a589b | 103 | } |
GermanD | 0:65c8fa8a589b | 104 | |
GermanD | 0:65c8fa8a589b | 105 | if (j==2) { |
GermanD | 0:65c8fa8a589b | 106 | ki=ki+wheel.getPulses(); |
GermanD | 0:65c8fa8a589b | 107 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 108 | if (ki>999){ |
GermanD | 0:65c8fa8a589b | 109 | ki=999; |
GermanD | 0:65c8fa8a589b | 110 | } |
GermanD | 0:65c8fa8a589b | 111 | if (ki<0){ |
GermanD | 0:65c8fa8a589b | 112 | ki=0; |
GermanD | 0:65c8fa8a589b | 113 | } |
GermanD | 0:65c8fa8a589b | 114 | lcd.locate(3,1); |
GermanD | 0:65c8fa8a589b | 115 | lcd.printf(" "); |
GermanD | 0:65c8fa8a589b | 116 | lcd.locate(3,1); |
GermanD | 0:65c8fa8a589b | 117 | lcd.printf("%d",ki); |
GermanD | 0:65c8fa8a589b | 118 | wait(0.2); |
GermanD | 0:65c8fa8a589b | 119 | |
GermanD | 0:65c8fa8a589b | 120 | if(botonEncoder.falling()){ |
GermanD | 0:65c8fa8a589b | 121 | j=3; |
GermanD | 0:65c8fa8a589b | 122 | led3=0; |
GermanD | 0:65c8fa8a589b | 123 | wait(0.3); |
GermanD | 0:65c8fa8a589b | 124 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 125 | } |
GermanD | 0:65c8fa8a589b | 126 | |
GermanD | 0:65c8fa8a589b | 127 | } |
GermanD | 0:65c8fa8a589b | 128 | |
GermanD | 0:65c8fa8a589b | 129 | if (j==3) { |
GermanD | 0:65c8fa8a589b | 130 | kd=kd+wheel.getPulses(); |
GermanD | 0:65c8fa8a589b | 131 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 132 | if (kd>999){ |
GermanD | 0:65c8fa8a589b | 133 | kd=999; |
GermanD | 0:65c8fa8a589b | 134 | } |
GermanD | 0:65c8fa8a589b | 135 | if (kd<0){ |
GermanD | 0:65c8fa8a589b | 136 | kd=0; |
GermanD | 0:65c8fa8a589b | 137 | } |
GermanD | 0:65c8fa8a589b | 138 | lcd.locate(11,1); |
GermanD | 0:65c8fa8a589b | 139 | lcd.printf(" "); |
GermanD | 0:65c8fa8a589b | 140 | lcd.locate(11,1); |
GermanD | 0:65c8fa8a589b | 141 | lcd.printf("%d",kd); |
GermanD | 0:65c8fa8a589b | 142 | wait(0.2); |
GermanD | 0:65c8fa8a589b | 143 | |
GermanD | 0:65c8fa8a589b | 144 | if(botonEncoder.falling()){ |
GermanD | 0:65c8fa8a589b | 145 | j=0; |
GermanD | 0:65c8fa8a589b | 146 | led3=0; |
GermanD | 0:65c8fa8a589b | 147 | wait(0.3); |
GermanD | 0:65c8fa8a589b | 148 | wheel.reset(); |
GermanD | 0:65c8fa8a589b | 149 | } |
GermanD | 0:65c8fa8a589b | 150 | |
GermanD | 0:65c8fa8a589b | 151 | } |
GermanD | 0:65c8fa8a589b | 152 | |
GermanD | 0:65c8fa8a589b | 153 | if (j==4) { |
GermanD | 0:65c8fa8a589b | 154 | j=0; |
GermanD | 0:65c8fa8a589b | 155 | } |
GermanD | 0:65c8fa8a589b | 156 | |
GermanD | 0:65c8fa8a589b | 157 | if (!button4){ |
GermanD | 0:65c8fa8a589b | 158 | break; //sale del bucle si pisan suiche4 |
GermanD | 0:65c8fa8a589b | 159 | } |
GermanD | 0:65c8fa8a589b | 160 | } //cierro while(1) |
GermanD | 0:65c8fa8a589b | 161 | |
GermanD | 0:65c8fa8a589b | 162 | //enter |
GermanD | 0:65c8fa8a589b | 163 | if(button4.falling()) |
GermanD | 0:65c8fa8a589b | 164 | { |
GermanD | 0:65c8fa8a589b | 165 | lcd.cls(); // al undir el boton 4 se borra la pantalla |
GermanD | 0:65c8fa8a589b | 166 | //for (int h=0;h<100;h++) |
GermanD | 0:65c8fa8a589b | 167 | //Vout= Vout+0.01; |
GermanD | 0:65c8fa8a589b | 168 | while(bandera==0) |
GermanD | 0:65c8fa8a589b | 169 | { |
GermanD | 0:65c8fa8a589b | 170 | wait(0.2); |
GermanD | 0:65c8fa8a589b | 171 | x=Vout.read(); // lee el voltaje de salida que nuestra analoga pero en porcentaje |
GermanD | 0:65c8fa8a589b | 172 | |
GermanD | 0:65c8fa8a589b | 173 | //} |
GermanD | 0:65c8fa8a589b | 174 | //pid |
GermanD | 0:65c8fa8a589b | 175 | |
GermanD | 0:65c8fa8a589b | 176 | err = sp-x; // Calcula el error |
GermanD | 0:65c8fa8a589b | 177 | pwmset = kp* err+ki * eInteg +kd* (err - ePrev); // ecuacion para el PID |
GermanD | 0:65c8fa8a589b | 178 | pwmset=(0.976801/5)*pwmset; // como el voltaje calculado fue un porcentaje se realiza una conversion respectiva al valor de salida |
GermanD | 0:65c8fa8a589b | 179 | Vout=pwmset; |
GermanD | 0:65c8fa8a589b | 180 | eInteg =eInteg+ err; // integral |
GermanD | 0:65c8fa8a589b | 181 | ePrev = err; |
GermanD | 0:65c8fa8a589b | 182 | lcd.locate(0,1); |
GermanD | 0:65c8fa8a589b | 183 | lcd.printf("error=%f",err); |
GermanD | 0:65c8fa8a589b | 184 | lcd.locate(1,0); |
GermanD | 0:65c8fa8a589b | 185 | lcd.printf("salida=%f",x); |
GermanD | 0:65c8fa8a589b | 186 | if (button4.falling()) |
GermanD | 0:65c8fa8a589b | 187 | { bandera=1; |
GermanD | 0:65c8fa8a589b | 188 | lcd.cls(); |
GermanD | 0:65c8fa8a589b | 189 | p=1; |
GermanD | 0:65c8fa8a589b | 190 | lcd.locate(8,0); |
GermanD | 0:65c8fa8a589b | 191 | lcd.printf("kp=%d",kp); |
GermanD | 0:65c8fa8a589b | 192 | lcd.locate(0,1); |
GermanD | 0:65c8fa8a589b | 193 | lcd.printf("Ki=%d", ki); |
GermanD | 0:65c8fa8a589b | 194 | lcd.locate(8,1); |
GermanD | 0:65c8fa8a589b | 195 | lcd.printf("Kd=%d", kd); |
GermanD | 0:65c8fa8a589b | 196 | lcd.locate(0,0); |
GermanD | 0:65c8fa8a589b | 197 | lcd.printf("Sp=%d", sp); |
GermanD | 0:65c8fa8a589b | 198 | |
GermanD | 0:65c8fa8a589b | 199 | } |
GermanD | 0:65c8fa8a589b | 200 | |
GermanD | 0:65c8fa8a589b | 201 | |
GermanD | 0:65c8fa8a589b | 202 | } |
GermanD | 0:65c8fa8a589b | 203 | bandera = 0; |
GermanD | 0:65c8fa8a589b | 204 | } |
GermanD | 0:65c8fa8a589b | 205 | |
GermanD | 0:65c8fa8a589b | 206 | |
GermanD | 0:65c8fa8a589b | 207 | |
GermanD | 0:65c8fa8a589b | 208 | } |
GermanD | 0:65c8fa8a589b | 209 | |
GermanD | 0:65c8fa8a589b | 210 | |
GermanD | 0:65c8fa8a589b | 211 |