ingreso de parametros a controlador PID con un ENCODER .
Dependencies: DebounceIn Debounced QEI TextLCD2 mbed
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?
User | Revision | Line number | New 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 |