Santiago Alvarez / Mbed 2 deprecated Encodertarea

d

Dependencies:   Debounced QEI mbed

main.cpp@1:e214227f5d00, 2013-12-12 (annotated)

Committer:
salvarear
Date:
Thu Dec 12 20:51:29 2013 +0000
Revision:
1:e214227f5d00
Parent:
0:93983293c81b 
d

Who changed what in which revision?

UserRevisionLine numberNew contents of line
salvarear 0:93983293c81b 1 #include "mbed.h"
salvarear 0:93983293c81b 2 #include "DebouncedIn.h"
salvarear 0:93983293c81b 3 #include "TextLCD.h"
salvarear 0:93983293c81b 4 #include "QEI.h"
salvarear 0:93983293c81b 5
salvarear 0:93983293c81b 6 AnalogIn Vin(PTB0); //entrada de la planta
salvarear 0:93983293c81b 7 AnalogOut Vout(PTE30); //salida hacia la planta
salvarear 0:93983293c81b 8
salvarear 1:e214227f5d00 9 QEI wheel (PTA16, PTA17, NC, 24); //distribucion de pines para el encoder.
salvarear 0:93983293c81b 10 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
salvarear 0:93983293c81b 11
salvarear 0:93983293c81b 12 DigitalOut led1(LED1);
salvarear 0:93983293c81b 13 DigitalOut led2(LED2);
salvarear 0:93983293c81b 14 DigitalOut led3(LED3);
salvarear 0:93983293c81b 15 DigitalOut led4(LED4);
salvarear 0:93983293c81b 16 DebouncedIn button1(PTC12); // Aumento
salvarear 0:93983293c81b 17 DebouncedIn button2(PTC13); // Disminución
salvarear 0:93983293c81b 18 DebouncedIn button3(PTC16); // Cambio de variable
salvarear 0:93983293c81b 19 DebouncedIn button4(PTC17); // Reset
salvarear 0:93983293c81b 20
salvarear 0:93983293c81b 21 int C1=0x0E; // solo muestra el curzor
salvarear 0:93983293c81b 22 int C2=0x18; // desplaza izquierda
salvarear 0:93983293c81b 23 int C3=0x1A; // desplaza derecha
salvarear 0:93983293c81b 24 int C4=0x0C; // quito cursor bajo
salvarear 1:e214227f5d00 25 int i;
salvarear 0:93983293c81b 26 int j,kp,ki,kd,sp,cont,medida,cont1,cont2,cont3;
salvarear 0:93983293c81b 27 float pid, err, ap, ai, ad, err_v, spf,kif,kpf,kdf,med;
salvarear 0:93983293c81b 28
salvarear 0:93983293c81b 29
salvarear 0:93983293c81b 30 int main() {
salvarear 0:93983293c81b 31
salvarear 0:93983293c81b 32 lcd.writeCommand(C1);
salvarear 0:93983293c81b 33 lcd.printf("Sp=0 Kp=0");
salvarear 0:93983293c81b 34 lcd.locate(0,1);
salvarear 0:93983293c81b 35 lcd.printf("Ki=0 Kd=0");
salvarear 0:93983293c81b 36 lcd.locate(2,0);
salvarear 0:93983293c81b 37 lcd.printf("=");
salvarear 0:93983293c81b 38
salvarear 0:93983293c81b 39 while(1) {
salvarear 0:93983293c81b 40
salvarear 1:e214227f5d00 41 cont1=wheel.getPulses(); //inicia el encoder
salvarear 0:93983293c81b 42 if(cont1<0){
salvarear 0:93983293c81b 43 wheel.reset();
salvarear 0:93983293c81b 44 cont1=0;
salvarear 0:93983293c81b 45 }
salvarear 0:93983293c81b 46 if(cont1!=cont2) {
salvarear 0:93983293c81b 47
salvarear 0:93983293c81b 48 switch(i) {
salvarear 0:93983293c81b 49
salvarear 0:93983293c81b 50 case 0:
salvarear 0:93983293c81b 51
salvarear 0:93983293c81b 52 lcd.locate(2,0);
salvarear 0:93983293c81b 53 lcd.printf("= ");
salvarear 0:93983293c81b 54 lcd.locate(3,0);
salvarear 0:93983293c81b 55 lcd.printf("%d",cont1);
salvarear 0:93983293c81b 56 sp=cont1;
salvarear 0:93983293c81b 57
salvarear 0:93983293c81b 58 break;
salvarear 0:93983293c81b 59 case 1:
salvarear 0:93983293c81b 60
salvarear 0:93983293c81b 61 lcd.locate(10,0);
salvarear 0:93983293c81b 62 lcd.printf("= ");
salvarear 0:93983293c81b 63 lcd.locate(11,0);
salvarear 0:93983293c81b 64 lcd.printf("%d",med);
salvarear 0:93983293c81b 65 kp=medida;
salvarear 0:93983293c81b 66 break;
salvarear 0:93983293c81b 67 case 2:
salvarear 0:93983293c81b 68 lcd.locate(2,1);
salvarear 0:93983293c81b 69 lcd.printf("= ");
salvarear 0:93983293c81b 70 lcd.locate(3,1);
salvarear 0:93983293c81b 71 lcd.printf("%d",med);
salvarear 0:93983293c81b 72 ki=med;
salvarear 0:93983293c81b 73 break;
salvarear 0:93983293c81b 74 case 3:
salvarear 0:93983293c81b 75 lcd.locate(10,1);
salvarear 0:93983293c81b 76 lcd.printf("= ");
salvarear 0:93983293c81b 77 lcd.locate(11,1);
salvarear 0:93983293c81b 78 lcd.printf("%d",med);
salvarear 0:93983293c81b 79 kd=med;
salvarear 0:93983293c81b 80 break;
salvarear 0:93983293c81b 81 }
salvarear 0:93983293c81b 82 cont2=cont1;
salvarear 0:93983293c81b 83
salvarear 0:93983293c81b 84 }
salvarear 0:93983293c81b 85
salvarear 0:93983293c81b 86
salvarear 1:e214227f5d00 87 if(button3.falling()) { //al oprimir el encoder
salvarear 0:93983293c81b 88 wheel.reset();
salvarear 0:93983293c81b 89 i++;
salvarear 0:93983293c81b 90 if(i>3) {
salvarear 0:93983293c81b 91 i=0;
salvarear 0:93983293c81b 92 }
salvarear 0:93983293c81b 93 switch (i) {
salvarear 0:93983293c81b 94 case 0:
salvarear 0:93983293c81b 95 lcd.locate(2,0);
salvarear 0:93983293c81b 96 lcd.printf("=");
salvarear 0:93983293c81b 97 break;
salvarear 0:93983293c81b 98 case 1:
salvarear 0:93983293c81b 99 lcd.locate(10,0);
salvarear 0:93983293c81b 100 lcd.printf("=");
salvarear 0:93983293c81b 101 break;
salvarear 0:93983293c81b 102 case 2:
salvarear 0:93983293c81b 103 lcd.locate(2,1);
salvarear 0:93983293c81b 104 lcd.printf("=");
salvarear 0:93983293c81b 105 break;
salvarear 0:93983293c81b 106 case 3:
salvarear 0:93983293c81b 107 lcd.locate(10,1);
salvarear 0:93983293c81b 108 lcd.printf("=");
salvarear 0:93983293c81b 109 break;
salvarear 0:93983293c81b 110 }
salvarear 0:93983293c81b 111 }
salvarear 0:93983293c81b 112
salvarear 0:93983293c81b 113
salvarear 0:93983293c81b 114 if (button4.falling()){
salvarear 0:93983293c81b 115 lcd.locate(0,1);
salvarear 0:93983293c81b 116 lcd.printf("Ha salido del programa ");
salvarear 1:e214227f5d00 117 break;
salvarear 0:93983293c81b 118 }
salvarear 0:93983293c81b 119
salvarear 0:93983293c81b 120 }
salvarear 0:93983293c81b 121
salvarear 0:93983293c81b 122 lcd.writeCommand(C4);
salvarear 0:93983293c81b 123 led3=1;
salvarear 0:93983293c81b 124 led3= !led3;
salvarear 0:93983293c81b 125 lcd.cls();
salvarear 0:93983293c81b 126 lcd.printf(" DATOS GUARDADOS!");
salvarear 0:93983293c81b 127 wait(0.8);
salvarear 0:93983293c81b 128 lcd.cls();
salvarear 0:93983293c81b 129 lcd.printf("INICIANDO EL PID");
salvarear 0:93983293c81b 130 wait(0.2);
salvarear 0:93983293c81b 131 lcd.cls();
salvarear 0:93983293c81b 132 lcd.printf("Er%d",err);
salvarear 0:93983293c81b 133 lcd.locate(8,0);
salvarear 0:93983293c81b 134 lcd.printf("Med%d",med);
salvarear 0:93983293c81b 135 lcd.locate(0,1);
salvarear 0:93983293c81b 136 lcd.printf("Sp%d",sp);
salvarear 0:93983293c81b 137 lcd.locate(8,1);
salvarear 0:93983293c81b 138 lcd.printf("pid%d",pid);
salvarear 0:93983293c81b 139 wait(0.5);
salvarear 0:93983293c81b 140
salvarear 0:93983293c81b 141 // CICLO PRINCIPAL CONTROLADOR PID
salvarear 0:93983293c81b 142 while(1) {
salvarear 1:e214227f5d00 143
salvarear 0:93983293c81b 144 med=Vin.read();
salvarear 0:93983293c81b 145 medida=med*100;
salvarear 0:93983293c81b 146 kpf=kp*0.001;
salvarear 0:93983293c81b 147 kif=ki*0.001;
salvarear 0:93983293c81b 148 kdf=kd*0.0001;
salvarear 0:93983293c81b 149 err = (sp-medida);
salvarear 0:93983293c81b 150
salvarear 0:93983293c81b 151 ap = kpf*err;
salvarear 1:e214227f5d00 152 ai =(kif*err)+ai; //calculo de la integral del error
salvarear 1:e214227f5d00 153 ad = kdf*(err-err_v); //calculo de la accion derivativa
salvarear 1:e214227f5d00 154 err_v=err; //guarda el error
salvarear 0:93983293c81b 155 pid = (ap+ai+ad);
salvarear 0:93983293c81b 156
salvarear 1:e214227f5d00 157
salvarear 0:93983293c81b 158 if (pid > 99){
salvarear 0:93983293c81b 159 pid=100;
salvarear 0:93983293c81b 160 }
salvarear 0:93983293c81b 161
salvarear 1:e214227f5d00 162
salvarear 0:93983293c81b 163 if (pid <0){
salvarear 0:93983293c81b 164 pid=0;
salvarear 0:93983293c81b 165 }
salvarear 0:93983293c81b 166
salvarear 1:e214227f5d00 167 wait(.5);
salvarear 0:93983293c81b 168
salvarear 0:93983293c81b 169
salvarear 0:93983293c81b 170
salvarear 0:93983293c81b 171 // se verifica que la accion integral no sea muy grande
salvarear 0:93983293c81b 172 if (ai > 999){
salvarear 0:93983293c81b 173 ai=1000;
salvarear 0:93983293c81b 174 }
salvarear 0:93983293c81b 175
salvarear 0:93983293c81b 176
salvarear 0:93983293c81b 177 Vout=pid;
salvarear 0:93983293c81b 178
salvarear 0:93983293c81b 179
salvarear 0:93983293c81b 180 lcd.locate(2,0);
salvarear 0:93983293c81b 181 lcd.printf(" ");
salvarear 0:93983293c81b 182 lcd.locate(0,0);
salvarear 0:93983293c81b 183 lcd.printf("Er%2.2f",err);
salvarear 0:93983293c81b 184 lcd.locate(10,0);
salvarear 0:93983293c81b 185 lcd.printf(" ");
salvarear 0:93983293c81b 186 lcd.locate(8,0);
salvarear 0:93983293c81b 187 lcd.printf("Me%d",medida);
salvarear 0:93983293c81b 188 lcd.locate(2,1);
salvarear 0:93983293c81b 189 lcd.printf(" ");
salvarear 0:93983293c81b 190 lcd.locate(0,1);
salvarear 0:93983293c81b 191 lcd.printf("Sp%d",sp);
salvarear 0:93983293c81b 192 lcd.locate(10,1);
salvarear 0:93983293c81b 193 lcd.printf(" ");
salvarear 0:93983293c81b 194 lcd.locate(8,1);
salvarear 0:93983293c81b 195 lcd.printf("Pid%4.2f",pid);
salvarear 0:93983293c81b 196 cont=0;
salvarear 0:93983293c81b 197 }
salvarear 0:93983293c81b 198 }
salvarear 0:93983293c81b 199
salvarear 0:93983293c81b 200
salvarear 0:93983293c81b 201