carlos ruiz / Mbed 2 deprecated tarea4encoder

ingreso de parametros a controlador PID con un ENCODER .

Dependencies:   DebounceIn Debounced QEI TextLCD2 mbed

/media/uploads/caaruizze/2013-12-07_08.00.53.jpg

main.cpp@3:c87c88124d37, 2013-11-20 (annotated)

Committer:
caaruizze
Date:
Wed Nov 20 21:54:28 2013 +0000
Revision:
3:c87c88124d37
Parent:
2:7fd0465280d2
Child:
4:ffdca5c64f59
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caaruizze 2:7fd0465280d2 1 #include "mbed.h"
aberk 0:bcff39fac858 2 #include "QEI.h"
caaruizze 2:7fd0465280d2 3 #include "TextLCD.h"
caaruizze 2:7fd0465280d2 4 #include "DebouncedIn.h"
caaruizze 2:7fd0465280d2 5 #include "DebounceIn.h"
caaruizze 2:7fd0465280d2 6 AnalogIn Vin(PTC2);
caaruizze 2:7fd0465280d2 7 AnalogOut Vo(PTE30);
caaruizze 2:7fd0465280d2 8 QEI wheel (PTD5, PTD0, NC, 624);
caaruizze 2:7fd0465280d2 9 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
caaruizze 3:c87c88124d37 10 DebouncedIn b1(PTC13);
caaruizze 2:7fd0465280d2 11 DebounceIn b2(PTC17);
caaruizze 2:7fd0465280d2 12 int x=1,i,d,f,g,h=0,m=0,l=0,k=0,j=0;
caaruizze 2:7fd0465280d2 13 int C1=0x0e;
caaruizze 2:7fd0465280d2 14 float kp, ki, kd, sp ,err, med, yr, pid, ap, ai, ad, err_v ,Sp;
caaruizze 2:7fd0465280d2 15 int main()
caaruizze 2:7fd0465280d2 16 {
caaruizze 2:7fd0465280d2 17 lcd.writeCommand(C1); // escribe el comando C1
caaruizze 2:7fd0465280d2 18 lcd.locate(0,0);
caaruizze 2:7fd0465280d2 19 lcd.printf("Sp:%d",i);
caaruizze 2:7fd0465280d2 20 lcd.locate(8,0);
caaruizze 2:7fd0465280d2 21 lcd.printf("Kp:%d",d);
caaruizze 2:7fd0465280d2 22 lcd.locate(0,1);
caaruizze 2:7fd0465280d2 23 lcd.printf("Ki:%d",f);
caaruizze 2:7fd0465280d2 24 lcd.locate(8,1);
caaruizze 2:7fd0465280d2 25 lcd.printf("Kd:%d",g);
aberk 0:bcff39fac858 26
caaruizze 2:7fd0465280d2 27 while(1) {
caaruizze 2:7fd0465280d2 28 j=wheel.getPulses();
caaruizze 2:7fd0465280d2 29 if(b1.falling())
caaruizze 2:7fd0465280d2 30 {
caaruizze 2:7fd0465280d2 31 x=x+1;
caaruizze 2:7fd0465280d2 32 }
caaruizze 2:7fd0465280d2 33 if(j<0)
caaruizze 2:7fd0465280d2 34 {
caaruizze 2:7fd0465280d2 35 wheel.reset();
caaruizze 2:7fd0465280d2 36 j=0;
caaruizze 2:7fd0465280d2 37 }
caaruizze 2:7fd0465280d2 38
caaruizze 2:7fd0465280d2 39 //condicionales para ubicación en pantalla y escritura
caaruizze 2:7fd0465280d2 40 if(x==1)
caaruizze 2:7fd0465280d2 41 {
caaruizze 2:7fd0465280d2 42 i=j+h;
caaruizze 2:7fd0465280d2 43 if(i<0)
caaruizze 2:7fd0465280d2 44 {
caaruizze 2:7fd0465280d2 45 i=0;
caaruizze 2:7fd0465280d2 46 }
caaruizze 2:7fd0465280d2 47 wait(0.1);
caaruizze 2:7fd0465280d2 48 lcd.locate(3,0);
caaruizze 2:7fd0465280d2 49 lcd.printf(" ");
caaruizze 2:7fd0465280d2 50 lcd.locate(3,0);
caaruizze 2:7fd0465280d2 51 lcd.printf("%d",i);
caaruizze 2:7fd0465280d2 52 }
caaruizze 2:7fd0465280d2 53 if(x==2)
caaruizze 2:7fd0465280d2 54 {
caaruizze 2:7fd0465280d2 55 d=(j)-(i)+h+m;
caaruizze 2:7fd0465280d2 56 if(d<0)
caaruizze 2:7fd0465280d2 57 {
caaruizze 2:7fd0465280d2 58 d=0;
caaruizze 2:7fd0465280d2 59 }
caaruizze 2:7fd0465280d2 60
caaruizze 2:7fd0465280d2 61 wait(0.1);
caaruizze 2:7fd0465280d2 62 lcd.locate(11,0);
caaruizze 2:7fd0465280d2 63 lcd.printf(" ");
caaruizze 2:7fd0465280d2 64 lcd.locate(11,0);
caaruizze 2:7fd0465280d2 65 lcd.printf("%d",d);
caaruizze 2:7fd0465280d2 66 }
caaruizze 2:7fd0465280d2 67 if(x==3)
caaruizze 2:7fd0465280d2 68 {
caaruizze 2:7fd0465280d2 69 f=j-d-i+h+m+l;
caaruizze 2:7fd0465280d2 70 if(f<0)
caaruizze 2:7fd0465280d2 71 {
caaruizze 2:7fd0465280d2 72 f=0;
caaruizze 2:7fd0465280d2 73 }
caaruizze 2:7fd0465280d2 74 wait(0.1);
caaruizze 2:7fd0465280d2 75 lcd.locate(3,1);
caaruizze 2:7fd0465280d2 76 lcd.printf(" ");
caaruizze 2:7fd0465280d2 77 lcd.locate(3,1);
caaruizze 2:7fd0465280d2 78 lcd.printf("%d",f);
caaruizze 2:7fd0465280d2 79 }
caaruizze 2:7fd0465280d2 80 if(x==4)
caaruizze 2:7fd0465280d2 81 {
caaruizze 2:7fd0465280d2 82 g=j-(i+d+f)+h+m+l+k;
caaruizze 2:7fd0465280d2 83 if(g<0)
caaruizze 2:7fd0465280d2 84 {
caaruizze 2:7fd0465280d2 85 g=0;
caaruizze 2:7fd0465280d2 86 }
caaruizze 2:7fd0465280d2 87 wait(0.1);
caaruizze 2:7fd0465280d2 88 lcd.locate(11,1);
caaruizze 2:7fd0465280d2 89 lcd.printf(" ");
caaruizze 2:7fd0465280d2 90 lcd.locate(11,1);
caaruizze 2:7fd0465280d2 91 lcd.printf("%d",g);
caaruizze 2:7fd0465280d2 92 }
caaruizze 2:7fd0465280d2 93 if(x>4)
caaruizze 2:7fd0465280d2 94 {
caaruizze 2:7fd0465280d2 95 x=1;
caaruizze 2:7fd0465280d2 96 h=i;
caaruizze 2:7fd0465280d2 97 m=d;
caaruizze 2:7fd0465280d2 98 l=f;
caaruizze 2:7fd0465280d2 99 k=g;
caaruizze 2:7fd0465280d2 100 wheel.reset();
caaruizze 2:7fd0465280d2 101 }
caaruizze 2:7fd0465280d2 102 if(b2.read()==0)
caaruizze 2:7fd0465280d2 103 {
caaruizze 2:7fd0465280d2 104 goto PID;
caaruizze 2:7fd0465280d2 105 }
caaruizze 2:7fd0465280d2 106 }//while
caaruizze 2:7fd0465280d2 107 PID:
caaruizze 2:7fd0465280d2 108
caaruizze 2:7fd0465280d2 109 wait(0.5); // espera 1 segundo
caaruizze 2:7fd0465280d2 110 lcd.cls(); // borra lcd
caaruizze 2:7fd0465280d2 111 wait(0.2);
caaruizze 2:7fd0465280d2 112 lcd.printf("DATOS GUARDADOS!");
caaruizze 2:7fd0465280d2 113 wait(1);
caaruizze 2:7fd0465280d2 114 lcd.cls();
caaruizze 2:7fd0465280d2 115
caaruizze 2:7fd0465280d2 116 lcd.locate(0,0);
caaruizze 2:7fd0465280d2 117 lcd.printf("Er:");
caaruizze 2:7fd0465280d2 118 lcd.locate(8,0);
caaruizze 2:7fd0465280d2 119 lcd.printf("Me:");
caaruizze 2:7fd0465280d2 120 lcd.locate(0,1);
caaruizze 2:7fd0465280d2 121 lcd.printf("Sp:");
caaruizze 2:7fd0465280d2 122 lcd.locate(8,1);
caaruizze 2:7fd0465280d2 123 lcd.printf("CO:");
caaruizze 2:7fd0465280d2 124
caaruizze 2:7fd0465280d2 125 while(1)
caaruizze 2:7fd0465280d2 126 {//whilePID
caaruizze 2:7fd0465280d2 127 wait(0.1);
caaruizze 3:c87c88124d37 128 sp=i;
caaruizze 3:c87c88124d37 129 kp=d;
caaruizze 3:c87c88124d37 130 kd=f;
caaruizze 2:7fd0465280d2 131 ki=g;
caaruizze 2:7fd0465280d2 132
caaruizze 2:7fd0465280d2 133 Sp=(sp*3.3)/100;
caaruizze 2:7fd0465280d2 134 med=Vin*3.3;
caaruizze 2:7fd0465280d2 135 err = (Sp-med);
caaruizze 2:7fd0465280d2 136 ap = kp*err;
caaruizze 2:7fd0465280d2 137 ai =(ki*err)+ai;
aberk 0:bcff39fac858 138
caaruizze 2:7fd0465280d2 139 ad = kd*(err-err_v);
caaruizze 2:7fd0465280d2 140 pid = (ap+ai+ad);
caaruizze 2:7fd0465280d2 141 if (pid<0)
caaruizze 2:7fd0465280d2 142 {
caaruizze 2:7fd0465280d2 143 pid=0;
caaruizze 2:7fd0465280d2 144 }
caaruizze 2:7fd0465280d2 145 if (pid>99)
caaruizze 2:7fd0465280d2 146 {
caaruizze 2:7fd0465280d2 147 pid=100;
caaruizze 2:7fd0465280d2 148 }
caaruizze 2:7fd0465280d2 149
caaruizze 2:7fd0465280d2 150 Vo=pid/100;
caaruizze 2:7fd0465280d2 151
caaruizze 2:7fd0465280d2 152
caaruizze 2:7fd0465280d2 153 lcd.locate(3,0);
caaruizze 2:7fd0465280d2 154 lcd.printf(" ");
caaruizze 2:7fd0465280d2 155 lcd.locate(3,0);
caaruizze 2:7fd0465280d2 156 lcd.printf("%.1f",err);
caaruizze 2:7fd0465280d2 157 lcd.locate(11,0);
caaruizze 2:7fd0465280d2 158 lcd.printf(" ");
caaruizze 2:7fd0465280d2 159 lcd.locate(11,0);
caaruizze 2:7fd0465280d2 160 lcd.printf("%.1f",med);
caaruizze 2:7fd0465280d2 161 lcd.locate(3,1);
caaruizze 2:7fd0465280d2 162 lcd.printf(" ");
caaruizze 2:7fd0465280d2 163 lcd.locate(3,1);
caaruizze 2:7fd0465280d2 164 lcd.printf("%.1f",Sp);
caaruizze 2:7fd0465280d2 165 lcd.locate(11,1);
caaruizze 2:7fd0465280d2 166 lcd.printf(" ");
caaruizze 2:7fd0465280d2 167 lcd.locate(11,1);
caaruizze 2:7fd0465280d2 168 lcd.printf("%.1f",pid);
caaruizze 2:7fd0465280d2 169
caaruizze 2:7fd0465280d2 170 err_v = err;
caaruizze 2:7fd0465280d2 171 }//whilePID
aberk 0:bcff39fac858 172
caaruizze 2:7fd0465280d2 173
caaruizze 2:7fd0465280d2 174 }//main