Sergio Londono / Mbed 2 deprecated Tarea4

encoder

Dependencies:   QEI TextLCD-modificada mbed

main.cpp@0:d7388a87aeb7, 2013-11-15 (annotated)

Committer:
salondonog
Date:
Fri Nov 15 17:45:21 2013 +0000
Revision:
0:d7388a87aeb7
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);
salondonog 0:d7388a87aeb7 12
salondonog 0:d7388a87aeb7 13 Timer t;
salondonog 0:d7388a87aeb7 14 int flag;
salondonog 0:d7388a87aeb7 15 int q;
salondonog 0:d7388a87aeb7 16 int C1=0x0E; //configurar el lcd para mostrar el guin bajo
salondonog 0:d7388a87aeb7 17 int C2=0x0C; //configurar el lcd para no mostrar el guion bajo
salondonog 0:d7388a87aeb7 18 int s=0; //set point
salondonog 0:d7388a87aeb7 19 int p=0; //ganancia proporcional
salondonog 0:d7388a87aeb7 20 int i=0; //ganancia integral
salondonog 0:d7388a87aeb7 21 int d=0; //ganancia derivativa
salondonog 0:d7388a87aeb7 22 int k=0; //variable de control}
salondonog 0:d7388a87aeb7 23 int err, med, yr, ap, ai, ad, err_v, cycle;
salondonog 0:d7388a87aeb7 24 int pid;
salondonog 0:d7388a87aeb7 25 int viejo=0;
salondonog 0:d7388a87aeb7 26 int cont=0;
salondonog 0:d7388a87aeb7 27
salondonog 0:d7388a87aeb7 28 void def_posicion(int h){
salondonog 0:d7388a87aeb7 29 if (h==0){
salondonog 0:d7388a87aeb7 30 lcd.locate(3,0); lcd.printf("%d",s);
salondonog 0:d7388a87aeb7 31 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 32 }
salondonog 0:d7388a87aeb7 33 else if(h==1){
salondonog 0:d7388a87aeb7 34 lcd.locate(11,0); lcd.printf("%d",p);
salondonog 0:d7388a87aeb7 35 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 36 }
salondonog 0:d7388a87aeb7 37 else if (h==2){
salondonog 0:d7388a87aeb7 38 lcd.locate(3,1); lcd.printf("%d",i);
salondonog 0:d7388a87aeb7 39 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 40 }
salondonog 0:d7388a87aeb7 41 else {
salondonog 0:d7388a87aeb7 42 lcd.locate(11,1); lcd.printf("%d",d);
salondonog 0:d7388a87aeb7 43 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 44 }
salondonog 0:d7388a87aeb7 45 }
salondonog 0:d7388a87aeb7 46
salondonog 0:d7388a87aeb7 47
salondonog 0:d7388a87aeb7 48 void incremento (int r) { //Funcion que incremente los valores
salondonog 0:d7388a87aeb7 49 if(k==0){
salondonog 0:d7388a87aeb7 50 s+=r;
salondonog 0:d7388a87aeb7 51 if(s<=0){
salondonog 0:d7388a87aeb7 52 s=0;
salondonog 0:d7388a87aeb7 53 }
salondonog 0:d7388a87aeb7 54 else if (s>=999){
salondonog 0:d7388a87aeb7 55 s=999;
salondonog 0:d7388a87aeb7 56 }
salondonog 0:d7388a87aeb7 57 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 58 lcd.printf("%d ",s);
salondonog 0:d7388a87aeb7 59 }
salondonog 0:d7388a87aeb7 60 else if(k==1){
salondonog 0:d7388a87aeb7 61 p+=r;
salondonog 0:d7388a87aeb7 62 if(p<=0){
salondonog 0:d7388a87aeb7 63 p=0;
salondonog 0:d7388a87aeb7 64 }
salondonog 0:d7388a87aeb7 65 else if (p>=999){
salondonog 0:d7388a87aeb7 66 p=999;
salondonog 0:d7388a87aeb7 67 }
salondonog 0:d7388a87aeb7 68 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 69 lcd.printf("%d ",p);
salondonog 0:d7388a87aeb7 70 }
salondonog 0:d7388a87aeb7 71 else if(k==2){
salondonog 0:d7388a87aeb7 72 i+=r;
salondonog 0:d7388a87aeb7 73 if(i<=0){
salondonog 0:d7388a87aeb7 74 i=0;
salondonog 0:d7388a87aeb7 75 }
salondonog 0:d7388a87aeb7 76 else if (i>=999){
salondonog 0:d7388a87aeb7 77 i=999;
salondonog 0:d7388a87aeb7 78 }
salondonog 0:d7388a87aeb7 79 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 80 lcd.printf("%d ",i);
salondonog 0:d7388a87aeb7 81 }
salondonog 0:d7388a87aeb7 82 else{
salondonog 0:d7388a87aeb7 83 d+=r;
salondonog 0:d7388a87aeb7 84 if(d<=0){
salondonog 0:d7388a87aeb7 85 d=0;
salondonog 0:d7388a87aeb7 86 }
salondonog 0:d7388a87aeb7 87 else if (d>=999){
salondonog 0:d7388a87aeb7 88 d=999;
salondonog 0:d7388a87aeb7 89 }
salondonog 0:d7388a87aeb7 90 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 91 lcd.printf("%d ",d);
salondonog 0:d7388a87aeb7 92 }
salondonog 0:d7388a87aeb7 93 }
salondonog 0:d7388a87aeb7 94
salondonog 0:d7388a87aeb7 95
salondonog 0:d7388a87aeb7 96 void star_patch1(void){ //funcion que imprime los caracteres que no van a variar en el display
salondonog 0:d7388a87aeb7 97 lcd.cls();
salondonog 0:d7388a87aeb7 98 lcd.locate(8,0);
salondonog 0:d7388a87aeb7 99 lcd.printf("Kp=%d",p);
salondonog 0:d7388a87aeb7 100 lcd.locate(0,1);
salondonog 0:d7388a87aeb7 101 lcd.printf("Ki=%d",i);
salondonog 0:d7388a87aeb7 102 lcd.locate(8,1);
salondonog 0:d7388a87aeb7 103 lcd.printf("Kd=%d",d);
salondonog 0:d7388a87aeb7 104 lcd.writeCommand(C1); //permite que el cursor se vea y sea intermitente
salondonog 0:d7388a87aeb7 105 lcd.locate(0,0);
salondonog 0:d7388a87aeb7 106 lcd.printf("Sp=%d",s);
salondonog 0:d7388a87aeb7 107 }
salondonog 0:d7388a87aeb7 108
salondonog 0:d7388a87aeb7 109 void star_patch2(void){ //funcion que imprime los caracteres que no van a variar en el display
salondonog 0:d7388a87aeb7 110 lcd.writeCommand(C2);
salondonog 0:d7388a87aeb7 111 lcd.cls();
salondonog 0:d7388a87aeb7 112 lcd.printf("***INICIA PID***"); lcd.printf("***************");
salondonog 0:d7388a87aeb7 113 wait(2);
salondonog 0:d7388a87aeb7 114 lcd.cls();
salondonog 0:d7388a87aeb7 115 lcd.printf("Er%=d",err);
salondonog 0:d7388a87aeb7 116 lcd.locate(8,0); lcd.printf("Me=%d",med);
salondonog 0:d7388a87aeb7 117 lcd.locate(0,1); lcd.printf("Sp=%d",s);
salondonog 0:d7388a87aeb7 118 lcd.locate(8,1); lcd.printf("Co=%d",pid);
salondonog 0:d7388a87aeb7 119 wait(3);
salondonog 0:d7388a87aeb7 120 }
salondonog 0:d7388a87aeb7 121
salondonog 0:d7388a87aeb7 122 int main(){
salondonog 0:d7388a87aeb7 123 star_patch1();
salondonog 0:d7388a87aeb7 124 viejo=0;
salondonog 0:d7388a87aeb7 125 cont=0;
salondonog 0:d7388a87aeb7 126 while(1){
salondonog 0:d7388a87aeb7 127 if(k>=3 && salto.falling()){
salondonog 0:d7388a87aeb7 128 k=0;
salondonog 0:d7388a87aeb7 129 def_posicion(k);
salondonog 0:d7388a87aeb7 130 }
salondonog 0:d7388a87aeb7 131 if(salto.falling()){
salondonog 0:d7388a87aeb7 132 ++k;
salondonog 0:d7388a87aeb7 133 def_posicion(k);
salondonog 0:d7388a87aeb7 134 }
salondonog 0:d7388a87aeb7 135 else if(set.falling()){
salondonog 0:d7388a87aeb7 136 break;
salondonog 0:d7388a87aeb7 137 }
salondonog 0:d7388a87aeb7 138 viejo=conteo.getPulses()-cont;
salondonog 0:d7388a87aeb7 139 cont=conteo.getPulses();
salondonog 0:d7388a87aeb7 140 if(viejo==0){
salondonog 0:d7388a87aeb7 141 //no hace nada
salondonog 0:d7388a87aeb7 142 }
salondonog 0:d7388a87aeb7 143 else {
salondonog 0:d7388a87aeb7 144 incremento(viejo);
salondonog 0:d7388a87aeb7 145 }}
salondonog 0:d7388a87aeb7 146
salondonog 0:d7388a87aeb7 147 star_patch2();
salondonog 0:d7388a87aeb7 148
salondonog 0:d7388a87aeb7 149 while (1){
salondonog 0:d7388a87aeb7 150
salondonog 0:d7388a87aeb7 151 med=Vin.read()*1000;
salondonog 0:d7388a87aeb7 152 err = (s-med);
salondonog 0:d7388a87aeb7 153 ap = k*err;
salondonog 0:d7388a87aeb7 154 ai =(i*err)+ai; //calculo de la integral del error
salondonog 0:d7388a87aeb7 155 // se verifica que la accion integral no sea muy grande
salondonog 0:d7388a87aeb7 156 ad = d*(err-err_v); //calculo de la accion derivativa
salondonog 0:d7388a87aeb7 157 pid = (ap+ai+ad);
salondonog 0:d7388a87aeb7 158 //****se muestran las variables******************************************
salondonog 0:d7388a87aeb7 159 if (pid > 999){
salondonog 0:d7388a87aeb7 160 pid=1000;
salondonog 0:d7388a87aeb7 161 }
salondonog 0:d7388a87aeb7 162 else if (pid<0){
salondonog 0:d7388a87aeb7 163 pid=0;
salondonog 0:d7388a87aeb7 164 }
salondonog 0:d7388a87aeb7 165 Vout.write(pid/1000);
salondonog 0:d7388a87aeb7 166 if(flag==0){
salondonog 0:d7388a87aeb7 167 t.start();
salondonog 0:d7388a87aeb7 168 flag=1;
salondonog 0:d7388a87aeb7 169 }
salondonog 0:d7388a87aeb7 170 if (t>0.3){
salondonog 0:d7388a87aeb7 171 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 172 lcd.printf(" ");
salondonog 0:d7388a87aeb7 173 lcd.locate(3,0);
salondonog 0:d7388a87aeb7 174 lcd.printf("%d",err);
salondonog 0:d7388a87aeb7 175 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 176 lcd.printf(" ");
salondonog 0:d7388a87aeb7 177 lcd.locate(11,0);
salondonog 0:d7388a87aeb7 178 lcd.printf("%d",med);
salondonog 0:d7388a87aeb7 179 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 180 lcd.printf(" ");
salondonog 0:d7388a87aeb7 181 lcd.locate(3,1);
salondonog 0:d7388a87aeb7 182 lcd.printf("%d",s);
salondonog 0:d7388a87aeb7 183 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 184 lcd.printf(" ");
salondonog 0:d7388a87aeb7 185 lcd.locate(11,1);
salondonog 0:d7388a87aeb7 186 lcd.printf("%d ",pid);
salondonog 0:d7388a87aeb7 187 t.reset();
salondonog 0:d7388a87aeb7 188 flag=0;
salondonog 0:d7388a87aeb7 189 wait(.1);
salondonog 0:d7388a87aeb7 190 }
salondonog 0:d7388a87aeb7 191 // se actualizan las variables *******************************************
salondonog 0:d7388a87aeb7 192 err_v = err;
salondonog 0:d7388a87aeb7 193 }
salondonog 0:d7388a87aeb7 194 }