Walter y Gregorio / Mbed 2 deprecated Pid_encoder_con_sonido

Tarea #4 encoder con sonido al utilizar el pulsador

Dependencies:   QEI TextLCD-modificada mbed

Fork of Tarea4 by Sergio Londono

main.cpp@1:35c783e6bc3b, 2014-04-09 (annotated)

Committer:
walterg
Date:
Wed Apr 09 04:32:17 2014 +0000
Revision:
1:35c783e6bc3b
Parent:
0:d7388a87aeb7 
Tarea #4 encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
salondonog 0:d7388a87aeb7 1 #include "mbed.h"
salondonog 0:d7388a87aeb7 2 #include "TextLCD.h"
salondonog 0:d7388a87aeb7 3 #include "DebouncedIn.h"
salondonog 0:d7388a87aeb7 4 #include "QEI.h"
salondonog 0:d7388a87aeb7 5
salondonog 0:d7388a87aeb7 6 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); //configura los puestos PTE0,PTE1,PTE2,PTE3,PTE4,PTE5, donde se conectara el LCD 16x2
salondonog 0:d7388a87aeb7 7 QEI conteo(PTA13,PTD5,NC,12);
salondonog 0:d7388a87aeb7 8 DebouncedIn salto(PTC16);
salondonog 0:d7388a87aeb7 9 DebouncedIn set(PTC17);
salondonog 0:d7388a87aeb7 10 AnalogIn Vin(PTB0);
salondonog 0:d7388a87aeb7 11 AnalogOut Vout(PTE30);
walterg 1:35c783e6bc3b 12 PwmOut pw(PTA12);
walterg 1:35c783e6bc3b 13 float Tdo=1/2093.005;
walterg 1:35c783e6bc3b 14 float Pdo=Tdo/2;
walterg 1:35c783e6bc3b 15 float Tre=1/2637.02;
walterg 1:35c783e6bc3b 16 float Pre=Tre/2;
walterg 1:35c783e6bc3b 17 float Tmi=1/3135.963;
walterg 1:35c783e6bc3b 18 float Pmi=Tmi/2;
salondonog 0:d7388a87aeb7 19 Timer t;
salondonog 0:d7388a87aeb7 20 int flag;
salondonog 0:d7388a87aeb7 21 int q;
salondonog 0:d7388a87aeb7 22 int C1=0x0E; //configurar el lcd para mostrar el guin bajo
salondonog 0:d7388a87aeb7 23 int C2=0x0C; //configurar el lcd para no mostrar el guion bajo
salondonog 0:d7388a87aeb7 24 int s=0; //set point
salondonog 0:d7388a87aeb7 25 int p=0; //ganancia proporcional
salondonog 0:d7388a87aeb7 26 int i=0; //ganancia integral
salondonog 0:d7388a87aeb7 27 int d=0; //ganancia derivativa
salondonog 0:d7388a87aeb7 28 int k=0; //variable de control}
salondonog 0:d7388a87aeb7 29 int err, med, yr, ap, ai, ad, err_v, cycle;
salondonog 0:d7388a87aeb7 30 int pid;
salondonog 0:d7388a87aeb7 31 int viejo=0;
salondonog 0:d7388a87aeb7 32 int cont=0;
salondonog 0:d7388a87aeb7 33
salondonog 0:d7388a87aeb7 34 void def_posicion(int h){
salondonog 0:d7388a87aeb7 35 if (h==0){
salondonog 0:d7388a87aeb7 36 lcd.locate(3,0); lcd.printf("%d",s);
salondonog 0:d7388a87aeb7 37 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 38 }
salondonog 0:d7388a87aeb7 39 else if(h==1){
salondonog 0:d7388a87aeb7 40 lcd.locate(11,0); lcd.printf("%d",p);
salondonog 0:d7388a87aeb7 41 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 42 }
salondonog 0:d7388a87aeb7 43 else if (h==2){
salondonog 0:d7388a87aeb7 44 lcd.locate(3,1); lcd.printf("%d",i);
salondonog 0:d7388a87aeb7 45 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 46 }
salondonog 0:d7388a87aeb7 47 else {
salondonog 0:d7388a87aeb7 48 lcd.locate(11,1); lcd.printf("%d",d);
salondonog 0:d7388a87aeb7 49 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 50 }
salondonog 0:d7388a87aeb7 51 }
salondonog 0:d7388a87aeb7 52
salondonog 0:d7388a87aeb7 53
salondonog 0:d7388a87aeb7 54 void incremento (int r) { //Funcion que incremente los valores
salondonog 0:d7388a87aeb7 55 if(k==0){
salondonog 0:d7388a87aeb7 56 s+=r;
salondonog 0:d7388a87aeb7 57 if(s<=0){
salondonog 0:d7388a87aeb7 58 s=0;
salondonog 0:d7388a87aeb7 59 }
salondonog 0:d7388a87aeb7 60 else if (s>=999){
salondonog 0:d7388a87aeb7 61 s=999;
salondonog 0:d7388a87aeb7 62 }
salondonog 0:d7388a87aeb7 63 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 64 lcd.printf("%d ",s);
salondonog 0:d7388a87aeb7 65 }
salondonog 0:d7388a87aeb7 66 else if(k==1){
salondonog 0:d7388a87aeb7 67 p+=r;
salondonog 0:d7388a87aeb7 68 if(p<=0){
salondonog 0:d7388a87aeb7 69 p=0;
salondonog 0:d7388a87aeb7 70 }
salondonog 0:d7388a87aeb7 71 else if (p>=999){
salondonog 0:d7388a87aeb7 72 p=999;
salondonog 0:d7388a87aeb7 73 }
salondonog 0:d7388a87aeb7 74 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 75 lcd.printf("%d ",p);
salondonog 0:d7388a87aeb7 76 }
salondonog 0:d7388a87aeb7 77 else if(k==2){
salondonog 0:d7388a87aeb7 78 i+=r;
salondonog 0:d7388a87aeb7 79 if(i<=0){
salondonog 0:d7388a87aeb7 80 i=0;
salondonog 0:d7388a87aeb7 81 }
salondonog 0:d7388a87aeb7 82 else if (i>=999){
salondonog 0:d7388a87aeb7 83 i=999;
salondonog 0:d7388a87aeb7 84 }
salondonog 0:d7388a87aeb7 85 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 86 lcd.printf("%d ",i);
salondonog 0:d7388a87aeb7 87 }
salondonog 0:d7388a87aeb7 88 else{
salondonog 0:d7388a87aeb7 89 d+=r;
salondonog 0:d7388a87aeb7 90 if(d<=0){
salondonog 0:d7388a87aeb7 91 d=0;
salondonog 0:d7388a87aeb7 92 }
salondonog 0:d7388a87aeb7 93 else if (d>=999){
salondonog 0:d7388a87aeb7 94 d=999;
salondonog 0:d7388a87aeb7 95 }
salondonog 0:d7388a87aeb7 96 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 97 lcd.printf("%d ",d);
salondonog 0:d7388a87aeb7 98 }
salondonog 0:d7388a87aeb7 99 }
salondonog 0:d7388a87aeb7 100
salondonog 0:d7388a87aeb7 101
salondonog 0:d7388a87aeb7 102 void star_patch1(void){ //funcion que imprime los caracteres que no van a variar en el display
salondonog 0:d7388a87aeb7 103 lcd.cls();
salondonog 0:d7388a87aeb7 104 lcd.locate(8,0);
salondonog 0:d7388a87aeb7 105 lcd.printf("Kp=%d",p);
salondonog 0:d7388a87aeb7 106 lcd.locate(0,1);
salondonog 0:d7388a87aeb7 107 lcd.printf("Ki=%d",i);
salondonog 0:d7388a87aeb7 108 lcd.locate(8,1);
salondonog 0:d7388a87aeb7 109 lcd.printf("Kd=%d",d);
salondonog 0:d7388a87aeb7 110 lcd.writeCommand(C1); //permite que el cursor se vea y sea intermitente
salondonog 0:d7388a87aeb7 111 lcd.locate(0,0);
salondonog 0:d7388a87aeb7 112 lcd.printf("Sp=%d",s);
salondonog 0:d7388a87aeb7 113 }
salondonog 0:d7388a87aeb7 114
salondonog 0:d7388a87aeb7 115 void star_patch2(void){ //funcion que imprime los caracteres que no van a variar en el display
salondonog 0:d7388a87aeb7 116 lcd.writeCommand(C2);
salondonog 0:d7388a87aeb7 117 lcd.cls();
salondonog 0:d7388a87aeb7 118 lcd.printf("***INICIA PID***"); lcd.printf("***************");
salondonog 0:d7388a87aeb7 119 wait(2);
salondonog 0:d7388a87aeb7 120 lcd.cls();
salondonog 0:d7388a87aeb7 121 lcd.printf("Er%=d",err);
salondonog 0:d7388a87aeb7 122 lcd.locate(8,0); lcd.printf("Me=%d",med);
salondonog 0:d7388a87aeb7 123 lcd.locate(0,1); lcd.printf("Sp=%d",s);
salondonog 0:d7388a87aeb7 124 lcd.locate(8,1); lcd.printf("Co=%d",pid);
salondonog 0:d7388a87aeb7 125 wait(3);
salondonog 0:d7388a87aeb7 126 }
salondonog 0:d7388a87aeb7 127
salondonog 0:d7388a87aeb7 128 int main(){
salondonog 0:d7388a87aeb7 129 star_patch1();
salondonog 0:d7388a87aeb7 130 viejo=0;
salondonog 0:d7388a87aeb7 131 cont=0;
salondonog 0:d7388a87aeb7 132 while(1){
salondonog 0:d7388a87aeb7 133 if(k>=3 && salto.falling()){
walterg 1:35c783e6bc3b 134 k=0;
walterg 1:35c783e6bc3b 135 pw.period(Tdo);
walterg 1:35c783e6bc3b 136 pw.pulsewidth(Pdo);
walterg 1:35c783e6bc3b 137 wait(0.1);
walterg 1:35c783e6bc3b 138 pw=0;
salondonog 0:d7388a87aeb7 139 def_posicion(k);
salondonog 0:d7388a87aeb7 140 }
salondonog 0:d7388a87aeb7 141 if(salto.falling()){
salondonog 0:d7388a87aeb7 142 ++k;
walterg 1:35c783e6bc3b 143 pw.period(Tdo);
walterg 1:35c783e6bc3b 144 pw.pulsewidth(Pdo);
walterg 1:35c783e6bc3b 145 wait(0.1);
walterg 1:35c783e6bc3b 146 pw=0;
salondonog 0:d7388a87aeb7 147 def_posicion(k);
salondonog 0:d7388a87aeb7 148 }
salondonog 0:d7388a87aeb7 149 else if(set.falling()){
salondonog 0:d7388a87aeb7 150 break;
salondonog 0:d7388a87aeb7 151 }
salondonog 0:d7388a87aeb7 152 viejo=conteo.getPulses()-cont;
salondonog 0:d7388a87aeb7 153 cont=conteo.getPulses();
salondonog 0:d7388a87aeb7 154 if(viejo==0){
salondonog 0:d7388a87aeb7 155 //no hace nada
salondonog 0:d7388a87aeb7 156 }
salondonog 0:d7388a87aeb7 157 else {
salondonog 0:d7388a87aeb7 158 incremento(viejo);
salondonog 0:d7388a87aeb7 159 }}
salondonog 0:d7388a87aeb7 160
salondonog 0:d7388a87aeb7 161 star_patch2();
salondonog 0:d7388a87aeb7 162
salondonog 0:d7388a87aeb7 163 while (1){
salondonog 0:d7388a87aeb7 164
salondonog 0:d7388a87aeb7 165 med=Vin.read()*1000;
salondonog 0:d7388a87aeb7 166 err = (s-med);
salondonog 0:d7388a87aeb7 167 ap = k*err;
salondonog 0:d7388a87aeb7 168 ai =(i*err)+ai; //calculo de la integral del error
salondonog 0:d7388a87aeb7 169 // se verifica que la accion integral no sea muy grande
salondonog 0:d7388a87aeb7 170 ad = d*(err-err_v); //calculo de la accion derivativa
salondonog 0:d7388a87aeb7 171 pid = (ap+ai+ad);
salondonog 0:d7388a87aeb7 172 //****se muestran las variables******************************************
salondonog 0:d7388a87aeb7 173 if (pid > 999){
salondonog 0:d7388a87aeb7 174 pid=1000;
salondonog 0:d7388a87aeb7 175 }
salondonog 0:d7388a87aeb7 176 else if (pid<0){
salondonog 0:d7388a87aeb7 177 pid=0;
salondonog 0:d7388a87aeb7 178 }
salondonog 0:d7388a87aeb7 179 Vout.write(pid/1000);
salondonog 0:d7388a87aeb7 180 if(flag==0){
salondonog 0:d7388a87aeb7 181 t.start();
salondonog 0:d7388a87aeb7 182 flag=1;
salondonog 0:d7388a87aeb7 183 }
salondonog 0:d7388a87aeb7 184 if (t>0.3){
salondonog 0:d7388a87aeb7 185 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 186 lcd.printf(" ");
salondonog 0:d7388a87aeb7 187 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 188 lcd.printf("%d",err);
salondonog 0:d7388a87aeb7 189 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 190 lcd.printf(" ");
salondonog 0:d7388a87aeb7 191 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 192 lcd.printf("%d",med);
salondonog 0:d7388a87aeb7 193 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 194 lcd.printf(" ");
salondonog 0:d7388a87aeb7 195 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 196 lcd.printf("%d",s);
salondonog 0:d7388a87aeb7 197 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 198 lcd.printf(" ");
salondonog 0:d7388a87aeb7 199 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 200 lcd.printf("%d ",pid);
salondonog 0:d7388a87aeb7 201 t.reset();
salondonog 0:d7388a87aeb7 202 flag=0;
salondonog 0:d7388a87aeb7 203 wait(.1);
salondonog 0:d7388a87aeb7 204 }
salondonog 0:d7388a87aeb7 205 // se actualizan las variables *******************************************
salondonog 0:d7388a87aeb7 206 err_v = err;
salondonog 0:d7388a87aeb7 207 }
salondonog 0:d7388a87aeb7 208 }