Mauricio Peña / Mbed 2 deprecated Encoder-PID-Tarea4

PID, donde las variaciones de SP, KP, KI y KD se producen por un encoder.

Dependencies:   Debounced QEI TextLCD11 mbed

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

Committer:
mandres7
Date:
Fri Nov 15 17:00:26 2013 +0000
Revision:
0:b83ad526e653
Tarea4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mandres7 0:b83ad526e653 1 #include "mbed.h"
mandres7 0:b83ad526e653 2 #include "TextLCD.h"
mandres7 0:b83ad526e653 3 #include "DebouncedIn.h"
mandres7 0:b83ad526e653 4 #include "QEI.h"
mandres7 0:b83ad526e653 5
mandres7 0:b83ad526e653 6 TextLCD lcd(PTC10, PTC11, PTC12, PTC13, PTC16, PTC17);
mandres7 0:b83ad526e653 7 DebouncedIn next(PTE5);
mandres7 0:b83ad526e653 8 DebouncedIn menu(PTE4);
mandres7 0:b83ad526e653 9 int C1=0x0E;
mandres7 0:b83ad526e653 10 int C4=0x0C;
mandres7 0:b83ad526e653 11 AnalogOut Aout(PTE30);
mandres7 0:b83ad526e653 12 AnalogIn Ain(PTC2);
mandres7 0:b83ad526e653 13 QEI leftQei(PTD6, PTD7, NC, 624);
mandres7 0:b83ad526e653 14
mandres7 0:b83ad526e653 15 int sp=0;
mandres7 0:b83ad526e653 16 int kp=0;
mandres7 0:b83ad526e653 17 int ki=0;
mandres7 0:b83ad526e653 18 int kd=0;
mandres7 0:b83ad526e653 19
mandres7 0:b83ad526e653 20 int b;
mandres7 0:b83ad526e653 21
mandres7 0:b83ad526e653 22 int c=0;
mandres7 0:b83ad526e653 23 int y=0;
mandres7 0:b83ad526e653 24 float i; // set point
mandres7 0:b83ad526e653 25 float j=0; // salida planta
mandres7 0:b83ad526e653 26 float ji=0; //variable proceso interno
mandres7 0:b83ad526e653 27 float spi=0; //variable proceso interno
mandres7 0:b83ad526e653 28 float h=0; // entrada planta
mandres7 0:b83ad526e653 29 float m=0; // Error
mandres7 0:b83ad526e653 30 float mv=0;
mandres7 0:b83ad526e653 31 float g=0;
mandres7 0:b83ad526e653 32 float x=0;
mandres7 0:b83ad526e653 33 float ap,ai,ad;
mandres7 0:b83ad526e653 34
mandres7 0:b83ad526e653 35 //NECESITO COMPARACIÓN Y YA
mandres7 0:b83ad526e653 36 int pulsos (int x){
mandres7 0:b83ad526e653 37
mandres7 0:b83ad526e653 38
mandres7 0:b83ad526e653 39 if (leftQei.getPulses()>b){
mandres7 0:b83ad526e653 40 x++;
mandres7 0:b83ad526e653 41 b=leftQei.getPulses();
mandres7 0:b83ad526e653 42 }
mandres7 0:b83ad526e653 43 if (leftQei.getPulses()<b){
mandres7 0:b83ad526e653 44 x--;
mandres7 0:b83ad526e653 45 b=leftQei.getPulses();
mandres7 0:b83ad526e653 46 if (x<0){
mandres7 0:b83ad526e653 47 x=0;
mandres7 0:b83ad526e653 48 }
mandres7 0:b83ad526e653 49 }
mandres7 0:b83ad526e653 50 return x;
mandres7 0:b83ad526e653 51 } // Fin Int main función pulsos
mandres7 0:b83ad526e653 52
mandres7 0:b83ad526e653 53 int main() {
mandres7 0:b83ad526e653 54 lcd.cls();
mandres7 0:b83ad526e653 55 lcd.printf("Sp: %d",sp);
mandres7 0:b83ad526e653 56 lcd.locate(8,0);
mandres7 0:b83ad526e653 57 lcd.printf("Kp: %d",kp);
mandres7 0:b83ad526e653 58 lcd.locate(0,1);
mandres7 0:b83ad526e653 59 lcd.printf("Ki: %d",ki);
mandres7 0:b83ad526e653 60 lcd.locate(8,1);
mandres7 0:b83ad526e653 61 lcd.printf("Kd: %d",kd);
mandres7 0:b83ad526e653 62 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
mandres7 0:b83ad526e653 63 lcd.locate(0,0);
mandres7 0:b83ad526e653 64 lcd.printf("Sp: %d",sp);
mandres7 0:b83ad526e653 65
mandres7 0:b83ad526e653 66
mandres7 0:b83ad526e653 67
mandres7 0:b83ad526e653 68 while(1) {
mandres7 0:b83ad526e653 69 if (next.falling()) {
mandres7 0:b83ad526e653 70 ++c;
mandres7 0:b83ad526e653 71 } //CAMBIO DE POSICIÓN ENCODER
mandres7 0:b83ad526e653 72 if (c==0){
mandres7 0:b83ad526e653 73 lcd.locate(3,0);
mandres7 0:b83ad526e653 74 lcd.putc(0xFE);
mandres7 0:b83ad526e653 75 lcd.locate(4,0);
mandres7 0:b83ad526e653 76 lcd.printf("%d",sp);
mandres7 0:b83ad526e653 77
mandres7 0:b83ad526e653 78 sp=pulsos(sp);//revisar si puedo dejar sp
mandres7 0:b83ad526e653 79
mandres7 0:b83ad526e653 80 //
mandres7 0:b83ad526e653 81
mandres7 0:b83ad526e653 82 if(sp>999){
mandres7 0:b83ad526e653 83 sp=999;
mandres7 0:b83ad526e653 84 }
mandres7 0:b83ad526e653 85
mandres7 0:b83ad526e653 86 //NEW !! (not zeros in -- process)
mandres7 0:b83ad526e653 87 if(sp<10){
mandres7 0:b83ad526e653 88 lcd.locate(5,0);
mandres7 0:b83ad526e653 89 lcd.putc(0xFE);
mandres7 0:b83ad526e653 90 }
mandres7 0:b83ad526e653 91
mandres7 0:b83ad526e653 92 if(sp<100){
mandres7 0:b83ad526e653 93 lcd.locate(6,0);
mandres7 0:b83ad526e653 94 lcd.putc(0xFE);
mandres7 0:b83ad526e653 95 }
mandres7 0:b83ad526e653 96
mandres7 0:b83ad526e653 97 lcd.locate(4,0);
mandres7 0:b83ad526e653 98 lcd.printf("%d", sp);
mandres7 0:b83ad526e653 99 //wait(0.1);
mandres7 0:b83ad526e653 100
mandres7 0:b83ad526e653 101 //
mandres7 0:b83ad526e653 102
mandres7 0:b83ad526e653 103 }
mandres7 0:b83ad526e653 104 if (c==1) {
mandres7 0:b83ad526e653 105 lcd.locate(11,0);
mandres7 0:b83ad526e653 106 lcd.putc(0xFE);
mandres7 0:b83ad526e653 107 lcd.locate(12,0);
mandres7 0:b83ad526e653 108 lcd.printf("%d",kp);
mandres7 0:b83ad526e653 109 kp=pulsos(kp); //revisar si puedo dejar kp
mandres7 0:b83ad526e653 110
mandres7 0:b83ad526e653 111 //
mandres7 0:b83ad526e653 112
mandres7 0:b83ad526e653 113 if(kp>999){
mandres7 0:b83ad526e653 114 kp=999;
mandres7 0:b83ad526e653 115 }
mandres7 0:b83ad526e653 116
mandres7 0:b83ad526e653 117 //NEW !! (not zeros in -- process)
mandres7 0:b83ad526e653 118 if(kp<10){
mandres7 0:b83ad526e653 119 lcd.locate(13,0);
mandres7 0:b83ad526e653 120 lcd.putc(0xFE);
mandres7 0:b83ad526e653 121 }
mandres7 0:b83ad526e653 122
mandres7 0:b83ad526e653 123 if(kp<100){
mandres7 0:b83ad526e653 124 lcd.locate(14,0);
mandres7 0:b83ad526e653 125 lcd.putc(0xFE);
mandres7 0:b83ad526e653 126 }
mandres7 0:b83ad526e653 127
mandres7 0:b83ad526e653 128 lcd.locate(12,0);
mandres7 0:b83ad526e653 129 lcd.printf("%d", kp);
mandres7 0:b83ad526e653 130 //wait(0.2);
mandres7 0:b83ad526e653 131
mandres7 0:b83ad526e653 132 //
mandres7 0:b83ad526e653 133
mandres7 0:b83ad526e653 134 }
mandres7 0:b83ad526e653 135 if (c==2) {
mandres7 0:b83ad526e653 136 lcd.locate(3,1);
mandres7 0:b83ad526e653 137 lcd.putc(0xFE);
mandres7 0:b83ad526e653 138 lcd.locate(4,1);
mandres7 0:b83ad526e653 139 lcd.printf("%d",ki);
mandres7 0:b83ad526e653 140 ki=pulsos(ki); //revisar si puedo dejar ki
mandres7 0:b83ad526e653 141
mandres7 0:b83ad526e653 142 //
mandres7 0:b83ad526e653 143
mandres7 0:b83ad526e653 144 if(ki>999){
mandres7 0:b83ad526e653 145 ki=999;
mandres7 0:b83ad526e653 146 }
mandres7 0:b83ad526e653 147
mandres7 0:b83ad526e653 148 //NEW !! (not zeros in -- process)
mandres7 0:b83ad526e653 149 if(ki<10){
mandres7 0:b83ad526e653 150 lcd.locate(5,1);
mandres7 0:b83ad526e653 151 lcd.putc(0xFE);
mandres7 0:b83ad526e653 152 }
mandres7 0:b83ad526e653 153
mandres7 0:b83ad526e653 154 if(ki<100){
mandres7 0:b83ad526e653 155 lcd.locate(6,1);
mandres7 0:b83ad526e653 156 lcd.putc(0xFE);
mandres7 0:b83ad526e653 157 }
mandres7 0:b83ad526e653 158
mandres7 0:b83ad526e653 159 lcd.locate(4,1);
mandres7 0:b83ad526e653 160 lcd.printf("%d", ki);
mandres7 0:b83ad526e653 161 //wait(0.2);
mandres7 0:b83ad526e653 162
mandres7 0:b83ad526e653 163 //
mandres7 0:b83ad526e653 164
mandres7 0:b83ad526e653 165 }
mandres7 0:b83ad526e653 166 if (c==3) {
mandres7 0:b83ad526e653 167 lcd.locate(11,1);
mandres7 0:b83ad526e653 168 lcd.putc(0xFE);
mandres7 0:b83ad526e653 169 lcd.locate(12,1);
mandres7 0:b83ad526e653 170 lcd.printf("%d",kd);
mandres7 0:b83ad526e653 171 kd=pulsos(kd); //revisar si puedo dejar kd
mandres7 0:b83ad526e653 172
mandres7 0:b83ad526e653 173 //
mandres7 0:b83ad526e653 174
mandres7 0:b83ad526e653 175 if(kd>999){
mandres7 0:b83ad526e653 176 kd=999;
mandres7 0:b83ad526e653 177 }
mandres7 0:b83ad526e653 178
mandres7 0:b83ad526e653 179 //NEW !! (not zeros in -- process)
mandres7 0:b83ad526e653 180 if(kd<10){
mandres7 0:b83ad526e653 181 lcd.locate(13,1);
mandres7 0:b83ad526e653 182 lcd.putc(0xFE);
mandres7 0:b83ad526e653 183 }
mandres7 0:b83ad526e653 184
mandres7 0:b83ad526e653 185 if(kd<100){
mandres7 0:b83ad526e653 186 lcd.locate(14,1);
mandres7 0:b83ad526e653 187 lcd.putc(0xFE);
mandres7 0:b83ad526e653 188 }
mandres7 0:b83ad526e653 189
mandres7 0:b83ad526e653 190 lcd.locate(12,1);
mandres7 0:b83ad526e653 191 lcd.printf("%d", kd);
mandres7 0:b83ad526e653 192 //wait(0.2);
mandres7 0:b83ad526e653 193 }
mandres7 0:b83ad526e653 194 if (c==4) {
mandres7 0:b83ad526e653 195 c=0;
mandres7 0:b83ad526e653 196 }
mandres7 0:b83ad526e653 197
mandres7 0:b83ad526e653 198 if (menu.falling()){
mandres7 0:b83ad526e653 199 break;
mandres7 0:b83ad526e653 200 }
mandres7 0:b83ad526e653 201 } //While
mandres7 0:b83ad526e653 202
mandres7 0:b83ad526e653 203 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
mandres7 0:b83ad526e653 204 lcd.cls(); //borra la pantalla
mandres7 0:b83ad526e653 205 lcd.printf(" GUARDADOS!");
mandres7 0:b83ad526e653 206 wait(2);
mandres7 0:b83ad526e653 207 lcd.cls();
mandres7 0:b83ad526e653 208 lcd.printf(" INICIA EL PID");
mandres7 0:b83ad526e653 209 wait(2);
mandres7 0:b83ad526e653 210 // se imprimen los parches del control *****************************************
mandres7 0:b83ad526e653 211
mandres7 0:b83ad526e653 212 i=sp/999;
mandres7 0:b83ad526e653 213 lcd.cls();
mandres7 0:b83ad526e653 214 lcd.printf("Er:%.2f",m);
mandres7 0:b83ad526e653 215 lcd.locate(8,0);
mandres7 0:b83ad526e653 216 lcd.printf("Me:%.2f",j);
mandres7 0:b83ad526e653 217 lcd.locate(0,1);
mandres7 0:b83ad526e653 218 lcd.printf("Sp:%d",sp);
mandres7 0:b83ad526e653 219 lcd.locate(8,1);
mandres7 0:b83ad526e653 220 lcd.printf("Co:%.1f",h);
mandres7 0:b83ad526e653 221 wait(2);
mandres7 0:b83ad526e653 222
mandres7 0:b83ad526e653 223 //
mandres7 0:b83ad526e653 224 while(1) {
mandres7 0:b83ad526e653 225
mandres7 0:b83ad526e653 226 spi=sp*(3.3/999);
mandres7 0:b83ad526e653 227
mandres7 0:b83ad526e653 228 //wait(0.3);
mandres7 0:b83ad526e653 229 i=sp/999;
mandres7 0:b83ad526e653 230 if(i<=1){
mandres7 0:b83ad526e653 231
mandres7 0:b83ad526e653 232 j=Ain;
mandres7 0:b83ad526e653 233 ji=j*3.3;
mandres7 0:b83ad526e653 234 m=(spi-ji);
mandres7 0:b83ad526e653 235 //n=m*100;
mandres7 0:b83ad526e653 236 ap=kp*m;
mandres7 0:b83ad526e653 237 ai=(ki*m)+ai;
mandres7 0:b83ad526e653 238
mandres7 0:b83ad526e653 239 ad=kd*(m-mv);
mandres7 0:b83ad526e653 240 h=ap+ai+ad;
mandres7 0:b83ad526e653 241
mandres7 0:b83ad526e653 242 if (h>999){
mandres7 0:b83ad526e653 243 h=999;
mandres7 0:b83ad526e653 244 }
mandres7 0:b83ad526e653 245 if (h<0){
mandres7 0:b83ad526e653 246 h=0;
mandres7 0:b83ad526e653 247 }
mandres7 0:b83ad526e653 248
mandres7 0:b83ad526e653 249 g=(h/999);
mandres7 0:b83ad526e653 250
mandres7 0:b83ad526e653 251 Aout=g;
mandres7 0:b83ad526e653 252 wait(0.22);
mandres7 0:b83ad526e653 253
mandres7 0:b83ad526e653 254
mandres7 0:b83ad526e653 255 lcd.cls();
mandres7 0:b83ad526e653 256 lcd.printf("Er:%.2f",m);
mandres7 0:b83ad526e653 257 lcd.locate(8,0);
mandres7 0:b83ad526e653 258 lcd.printf("Me:%.2f",j);
mandres7 0:b83ad526e653 259 lcd.locate(0,1);
mandres7 0:b83ad526e653 260 lcd.printf("Sp:%d",sp);
mandres7 0:b83ad526e653 261 lcd.locate(8,1);
mandres7 0:b83ad526e653 262 lcd.printf("Co:%.1f",h);
mandres7 0:b83ad526e653 263 }
mandres7 0:b83ad526e653 264
mandres7 0:b83ad526e653 265
mandres7 0:b83ad526e653 266 mv=m;
mandres7 0:b83ad526e653 267 } //While
mandres7 0:b83ad526e653 268 //
mandres7 0:b83ad526e653 269
mandres7 0:b83ad526e653 270 }//int main