Mauricio Peña / Mbed 2 deprecated PID_Encoder

Cuarta Tarea

Dependencies:   Debounced QEI TextLCD11 mbed

main.cpp@0:fcae7980d3cf, 2013-11-10 (annotated)

Committer:
mandres7
Date:
Sun Nov 10 21:41:22 2013 +0000
Revision:
0:fcae7980d3cf
Cuarta tarea procesadores

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mandres7 0:fcae7980d3cf 1 #include "mbed.h"
mandres7 0:fcae7980d3cf 2 #include "TextLCD.h"
mandres7 0:fcae7980d3cf 3 #include "DebouncedIn.h"
mandres7 0:fcae7980d3cf 4 #include "QEI.h"
mandres7 0:fcae7980d3cf 5
mandres7 0:fcae7980d3cf 6 TextLCD lcd(PTC10, PTC11, PTC12, PTC13, PTC16, PTC17); // rs, e, d4-d7.. Nota: No se quiere función de lectura, luego ese puerto a tierra.
mandres7 0:fcae7980d3cf 7 //lectura=1, escritura=0
mandres7 0:fcae7980d3cf 8 QEI leftQei(PTD6, PTD7, NC, 624);
mandres7 0:fcae7980d3cf 9 //Encoder Sp, Kp, Ki, Kd
mandres7 0:fcae7980d3cf 10 PwmOut rled(LED_GREEN);
mandres7 0:fcae7980d3cf 11
mandres7 0:fcae7980d3cf 12 DebouncedIn next(PTE5);
mandres7 0:fcae7980d3cf 13 AnalogIn Ain(PTC2);
mandres7 0:fcae7980d3cf 14 AnalogOut Aout(PTE30);
mandres7 0:fcae7980d3cf 15
mandres7 0:fcae7980d3cf 16 int C1=0x0F;
mandres7 0:fcae7980d3cf 17 int i,j,k,l;
mandres7 0:fcae7980d3cf 18 float ap,ai,ad;
mandres7 0:fcae7980d3cf 19
mandres7 0:fcae7980d3cf 20 float p; // set point
mandres7 0:fcae7980d3cf 21 float s=0; // salida planta
mandres7 0:fcae7980d3cf 22 float h=0; // entrada planta
mandres7 0:fcae7980d3cf 23 float m=0; // Error
mandres7 0:fcae7980d3cf 24 float mv=0;
mandres7 0:fcae7980d3cf 25 float g=0;
mandres7 0:fcae7980d3cf 26 float x=0;
mandres7 0:fcae7980d3cf 27
mandres7 0:fcae7980d3cf 28 int main() {
mandres7 0:fcae7980d3cf 29
mandres7 0:fcae7980d3cf 30 int c;
mandres7 0:fcae7980d3cf 31 c=0;
mandres7 0:fcae7980d3cf 32
mandres7 0:fcae7980d3cf 33 lcd.cls();
mandres7 0:fcae7980d3cf 34 lcd.locate(0,0);
mandres7 0:fcae7980d3cf 35 lcd.printf("sp:%d",i);
mandres7 0:fcae7980d3cf 36 lcd.locate(8,0);
mandres7 0:fcae7980d3cf 37 lcd.printf("kp:%d",j);
mandres7 0:fcae7980d3cf 38 lcd.locate(0,1);
mandres7 0:fcae7980d3cf 39 lcd.printf("ki:%d",k);
mandres7 0:fcae7980d3cf 40 lcd.locate(8,1);
mandres7 0:fcae7980d3cf 41 lcd.printf("kd:%d",l);
mandres7 0:fcae7980d3cf 42
mandres7 0:fcae7980d3cf 43 //SP
mandres7 0:fcae7980d3cf 44
mandres7 0:fcae7980d3cf 45 while (1){
mandres7 0:fcae7980d3cf 46
mandres7 0:fcae7980d3cf 47 if (next.falling()){
mandres7 0:fcae7980d3cf 48 c++;
mandres7 0:fcae7980d3cf 49 }
mandres7 0:fcae7980d3cf 50
mandres7 0:fcae7980d3cf 51 //Salto a SP
mandres7 0:fcae7980d3cf 52 if (c==0){
mandres7 0:fcae7980d3cf 53
mandres7 0:fcae7980d3cf 54 lcd.locate(3,0);
mandres7 0:fcae7980d3cf 55 lcd.putc(0xFE);
mandres7 0:fcae7980d3cf 56 lcd.writeCommand(C1);
mandres7 0:fcae7980d3cf 57 lcd.locate(3,0);
mandres7 0:fcae7980d3cf 58 lcd.printf("%d",i);
mandres7 0:fcae7980d3cf 59
mandres7 0:fcae7980d3cf 60 while(c==0){
mandres7 0:fcae7980d3cf 61
mandres7 0:fcae7980d3cf 62 //espacio de trabajo
mandres7 0:fcae7980d3cf 63
mandres7 0:fcae7980d3cf 64 i=leftQei.getPulses();
mandres7 0:fcae7980d3cf 65 //
mandres7 0:fcae7980d3cf 66
mandres7 0:fcae7980d3cf 67
mandres7 0:fcae7980d3cf 68 lcd.locate(3,0);
mandres7 0:fcae7980d3cf 69 lcd.printf("%i",leftQei.getPulses());
mandres7 0:fcae7980d3cf 70 wait(0.2);
mandres7 0:fcae7980d3cf 71
mandres7 0:fcae7980d3cf 72
mandres7 0:fcae7980d3cf 73 //fin espacio de trabajo
mandres7 0:fcae7980d3cf 74
mandres7 0:fcae7980d3cf 75 if(next.falling()){
mandres7 0:fcae7980d3cf 76 c++;
mandres7 0:fcae7980d3cf 77 }
mandres7 0:fcae7980d3cf 78 }
mandres7 0:fcae7980d3cf 79 }
mandres7 0:fcae7980d3cf 80
mandres7 0:fcae7980d3cf 81
mandres7 0:fcae7980d3cf 82 //Salto a Kp
mandres7 0:fcae7980d3cf 83 if (c==1){
mandres7 0:fcae7980d3cf 84
mandres7 0:fcae7980d3cf 85 lcd.locate(11,0);
mandres7 0:fcae7980d3cf 86 lcd.putc(0xFE);
mandres7 0:fcae7980d3cf 87 lcd.writeCommand(C1);
mandres7 0:fcae7980d3cf 88 lcd.locate(11,0);
mandres7 0:fcae7980d3cf 89 lcd.printf("%d",j);
mandres7 0:fcae7980d3cf 90
mandres7 0:fcae7980d3cf 91 while(c==1){
mandres7 0:fcae7980d3cf 92
mandres7 0:fcae7980d3cf 93 //Work space
mandres7 0:fcae7980d3cf 94
mandres7 0:fcae7980d3cf 95 j=leftQei.getPulses();
mandres7 0:fcae7980d3cf 96
mandres7 0:fcae7980d3cf 97 //
mandres7 0:fcae7980d3cf 98
mandres7 0:fcae7980d3cf 99 lcd.locate(11,0);
mandres7 0:fcae7980d3cf 100 lcd.printf("%i",leftQei.getPulses());
mandres7 0:fcae7980d3cf 101 wait(0.2);
mandres7 0:fcae7980d3cf 102
mandres7 0:fcae7980d3cf 103 //End work space
mandres7 0:fcae7980d3cf 104
mandres7 0:fcae7980d3cf 105 if(next.falling()){
mandres7 0:fcae7980d3cf 106 c++;
mandres7 0:fcae7980d3cf 107 }
mandres7 0:fcae7980d3cf 108 }
mandres7 0:fcae7980d3cf 109 }
mandres7 0:fcae7980d3cf 110
mandres7 0:fcae7980d3cf 111 //Salto a Kp
mandres7 0:fcae7980d3cf 112 if (c==2){
mandres7 0:fcae7980d3cf 113
mandres7 0:fcae7980d3cf 114 lcd.locate(3,1);
mandres7 0:fcae7980d3cf 115 lcd.putc(0xFE);
mandres7 0:fcae7980d3cf 116 lcd.writeCommand(C1);
mandres7 0:fcae7980d3cf 117 lcd.locate(3,1);
mandres7 0:fcae7980d3cf 118 lcd.printf("%d",k);
mandres7 0:fcae7980d3cf 119
mandres7 0:fcae7980d3cf 120 while(c==2){
mandres7 0:fcae7980d3cf 121
mandres7 0:fcae7980d3cf 122 //Work space
mandres7 0:fcae7980d3cf 123
mandres7 0:fcae7980d3cf 124 k=leftQei.getPulses();
mandres7 0:fcae7980d3cf 125
mandres7 0:fcae7980d3cf 126 //
mandres7 0:fcae7980d3cf 127
mandres7 0:fcae7980d3cf 128 lcd.locate(3,1);
mandres7 0:fcae7980d3cf 129 lcd.printf("%i",leftQei.getPulses());
mandres7 0:fcae7980d3cf 130 wait(0.2);
mandres7 0:fcae7980d3cf 131
mandres7 0:fcae7980d3cf 132 //End work space
mandres7 0:fcae7980d3cf 133
mandres7 0:fcae7980d3cf 134 if(next.falling()){
mandres7 0:fcae7980d3cf 135 c++;
mandres7 0:fcae7980d3cf 136 }
mandres7 0:fcae7980d3cf 137 }
mandres7 0:fcae7980d3cf 138 }
mandres7 0:fcae7980d3cf 139
mandres7 0:fcae7980d3cf 140 //Salto a Kp
mandres7 0:fcae7980d3cf 141 if (c==3){
mandres7 0:fcae7980d3cf 142
mandres7 0:fcae7980d3cf 143 lcd.locate(11,1);
mandres7 0:fcae7980d3cf 144 lcd.putc(0xFE);
mandres7 0:fcae7980d3cf 145 lcd.writeCommand(C1);
mandres7 0:fcae7980d3cf 146 lcd.locate(11,1);
mandres7 0:fcae7980d3cf 147 lcd.printf("%d",l);
mandres7 0:fcae7980d3cf 148
mandres7 0:fcae7980d3cf 149 while(c==3){
mandres7 0:fcae7980d3cf 150
mandres7 0:fcae7980d3cf 151 //Work space
mandres7 0:fcae7980d3cf 152
mandres7 0:fcae7980d3cf 153 l=leftQei.getPulses();
mandres7 0:fcae7980d3cf 154
mandres7 0:fcae7980d3cf 155 //
mandres7 0:fcae7980d3cf 156
mandres7 0:fcae7980d3cf 157 lcd.locate(11,1);
mandres7 0:fcae7980d3cf 158 lcd.printf("%i",leftQei.getPulses());
mandres7 0:fcae7980d3cf 159 wait(0.2);
mandres7 0:fcae7980d3cf 160
mandres7 0:fcae7980d3cf 161
mandres7 0:fcae7980d3cf 162 //End work space
mandres7 0:fcae7980d3cf 163
mandres7 0:fcae7980d3cf 164 if(next.falling()){
mandres7 0:fcae7980d3cf 165 c++;
mandres7 0:fcae7980d3cf 166 }
mandres7 0:fcae7980d3cf 167 }
mandres7 0:fcae7980d3cf 168 }
mandres7 0:fcae7980d3cf 169
mandres7 0:fcae7980d3cf 170 //
mandres7 0:fcae7980d3cf 171 if(c==4 || c>4){
mandres7 0:fcae7980d3cf 172 c=0;
mandres7 0:fcae7980d3cf 173 }
mandres7 0:fcae7980d3cf 174
mandres7 0:fcae7980d3cf 175 }
mandres7 0:fcae7980d3cf 176
mandres7 0:fcae7980d3cf 177 //Borrar
mandres7 0:fcae7980d3cf 178
mandres7 0:fcae7980d3cf 179 lcd.cls();
mandres7 0:fcae7980d3cf 180 lcd.printf("Er%d",m);
mandres7 0:fcae7980d3cf 181 lcd.locate(8,0);
mandres7 0:fcae7980d3cf 182 lcd.printf("Me%d",s);
mandres7 0:fcae7980d3cf 183 lcd.locate(0,1);
mandres7 0:fcae7980d3cf 184 lcd.printf("Sp%d",i);
mandres7 0:fcae7980d3cf 185 lcd.locate(8,1);
mandres7 0:fcae7980d3cf 186 lcd.printf("Co%d",x);
mandres7 0:fcae7980d3cf 187 wait(5);
mandres7 0:fcae7980d3cf 188
mandres7 0:fcae7980d3cf 189 //Ahora empieza el PID
mandres7 0:fcae7980d3cf 190
mandres7 0:fcae7980d3cf 191 p=i/100;
mandres7 0:fcae7980d3cf 192
mandres7 0:fcae7980d3cf 193 while(1) {
mandres7 0:fcae7980d3cf 194 wait(0.3);
mandres7 0:fcae7980d3cf 195
mandres7 0:fcae7980d3cf 196 if(p<0.999){
mandres7 0:fcae7980d3cf 197
mandres7 0:fcae7980d3cf 198 s=Ain;
mandres7 0:fcae7980d3cf 199 m=(p-s);
mandres7 0:fcae7980d3cf 200 //n=m*100;
mandres7 0:fcae7980d3cf 201 ap=j*m;
mandres7 0:fcae7980d3cf 202 ai=(k*m)+ai;
mandres7 0:fcae7980d3cf 203
mandres7 0:fcae7980d3cf 204 if (ai>999){
mandres7 0:fcae7980d3cf 205 ai=998;
mandres7 0:fcae7980d3cf 206 }
mandres7 0:fcae7980d3cf 207
mandres7 0:fcae7980d3cf 208 ad=l*(m-mv);
mandres7 0:fcae7980d3cf 209 h=ap+ai+ad;
mandres7 0:fcae7980d3cf 210 x=h/100;
mandres7 0:fcae7980d3cf 211 g=(h*3.3/100);
mandres7 0:fcae7980d3cf 212
mandres7 0:fcae7980d3cf 213 if (h<0){
mandres7 0:fcae7980d3cf 214 h=0;
mandres7 0:fcae7980d3cf 215 }
mandres7 0:fcae7980d3cf 216
mandres7 0:fcae7980d3cf 217
mandres7 0:fcae7980d3cf 218
mandres7 0:fcae7980d3cf 219 Aout=g;
mandres7 0:fcae7980d3cf 220 wait(0.22);
mandres7 0:fcae7980d3cf 221
mandres7 0:fcae7980d3cf 222
mandres7 0:fcae7980d3cf 223 lcd.cls();
mandres7 0:fcae7980d3cf 224 lcd.printf("Er:%.3f",m);
mandres7 0:fcae7980d3cf 225 lcd.locate(8,0);
mandres7 0:fcae7980d3cf 226 lcd.printf("Me:%.3f",s);
mandres7 0:fcae7980d3cf 227 lcd.locate(0,1);
mandres7 0:fcae7980d3cf 228 lcd.printf("Sp:%.2f",p);
mandres7 0:fcae7980d3cf 229 lcd.locate(8,1);
mandres7 0:fcae7980d3cf 230 lcd.printf("Co:%.3f",x);
mandres7 0:fcae7980d3cf 231 }
mandres7 0:fcae7980d3cf 232
mandres7 0:fcae7980d3cf 233
mandres7 0:fcae7980d3cf 234 mv=m;
mandres7 0:fcae7980d3cf 235
mandres7 0:fcae7980d3cf 236 }
mandres7 0:fcae7980d3cf 237 }
mandres7 0:fcae7980d3cf 238
mandres7 0:fcae7980d3cf 239
mandres7 0:fcae7980d3cf 240