Laura Corrales / Mbed 2 deprecated Tarea4_Encoder

This program makes a PID control and the values por control action are entered by quadrature encoder.

Dependencies:   DebouncedIn QEI TextLCD mbed

main.cpp@0:8e9ddfd2cbe2, 2013-12-15 (annotated)

Committer:
lcorralesc1
Date:
Sun Dec 15 16:18:35 2013 +0000
Revision:
0:8e9ddfd2cbe2
This program makes a PID control and the values por control action are entered by quadrature encoder.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lcorralesc1 0:8e9ddfd2cbe2 1 #include "mbed.h"
lcorralesc1 0:8e9ddfd2cbe2 2 #include "DebouncedIn.h"
lcorralesc1 0:8e9ddfd2cbe2 3 #include "TextLCD.h"
lcorralesc1 0:8e9ddfd2cbe2 4 #include "QEI.h"
lcorralesc1 0:8e9ddfd2cbe2 5
lcorralesc1 0:8e9ddfd2cbe2 6
lcorralesc1 0:8e9ddfd2cbe2 7 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
lcorralesc1 0:8e9ddfd2cbe2 8 QEI wheel (PTA1, PTA2, NC, 50);
lcorralesc1 0:8e9ddfd2cbe2 9
lcorralesc1 0:8e9ddfd2cbe2 10 AnalogIn y(PTC2);
lcorralesc1 0:8e9ddfd2cbe2 11 AnalogOut u(PTE30);
lcorralesc1 0:8e9ddfd2cbe2 12
lcorralesc1 0:8e9ddfd2cbe2 13 DigitalOut led1(LED1);
lcorralesc1 0:8e9ddfd2cbe2 14 DigitalOut led2(LED2);
lcorralesc1 0:8e9ddfd2cbe2 15 DigitalOut led3(LED3);
lcorralesc1 0:8e9ddfd2cbe2 16 DebouncedIn button1(PTC7);
lcorralesc1 0:8e9ddfd2cbe2 17 DebouncedIn button2(PTC13);
lcorralesc1 0:8e9ddfd2cbe2 18 DebouncedIn button3(PTC0);
lcorralesc1 0:8e9ddfd2cbe2 19 DebouncedIn button4(PTC17);
lcorralesc1 0:8e9ddfd2cbe2 20
lcorralesc1 0:8e9ddfd2cbe2 21 //codigos movimiento del curzor
lcorralesc1 0:8e9ddfd2cbe2 22 //18 para izquierda
lcorralesc1 0:8e9ddfd2cbe2 23 //1A para derecha
lcorralesc1 0:8e9ddfd2cbe2 24
lcorralesc1 0:8e9ddfd2cbe2 25 //int C1=0x0E; // solo muestra el curzor
lcorralesc1 0:8e9ddfd2cbe2 26 int C2=0x18; // desplaza izquierda
lcorralesc1 0:8e9ddfd2cbe2 27 int C3=0x1A; // desplaza derecha
lcorralesc1 0:8e9ddfd2cbe2 28 int C4=0x0C; // quito cursor bajo
lcorralesc1 0:8e9ddfd2cbe2 29 int C1=0x0F;
lcorralesc1 0:8e9ddfd2cbe2 30
lcorralesc1 0:8e9ddfd2cbe2 31 int i; // indice de la variable
lcorralesc1 0:8e9ddfd2cbe2 32 int j; //variable controla cambio 4 posiciones
lcorralesc1 0:8e9ddfd2cbe2 33 int kp, ki, kd, sp, med2, yr, cycle, c;
lcorralesc1 0:8e9ddfd2cbe2 34 float pid, med, ap, ai, ad, err, err_v;
lcorralesc1 0:8e9ddfd2cbe2 35
lcorralesc1 0:8e9ddfd2cbe2 36 int main() {
lcorralesc1 0:8e9ddfd2cbe2 37 lcd.cls();
lcorralesc1 0:8e9ddfd2cbe2 38 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
lcorralesc1 0:8e9ddfd2cbe2 39 lcd.locate(0,0);
lcorralesc1 0:8e9ddfd2cbe2 40 lcd.printf("Sp%d",sp);
lcorralesc1 0:8e9ddfd2cbe2 41 lcd.locate(8,0);
lcorralesc1 0:8e9ddfd2cbe2 42 lcd.printf("Kp%d",kp);
lcorralesc1 0:8e9ddfd2cbe2 43 lcd.locate(0,1);
lcorralesc1 0:8e9ddfd2cbe2 44 lcd.printf("Ki%d",ki);
lcorralesc1 0:8e9ddfd2cbe2 45 lcd.locate(8,1);
lcorralesc1 0:8e9ddfd2cbe2 46 lcd.printf("Kd%d",kd);
lcorralesc1 0:8e9ddfd2cbe2 47
lcorralesc1 0:8e9ddfd2cbe2 48
lcorralesc1 0:8e9ddfd2cbe2 49 while(1) {
lcorralesc1 0:8e9ddfd2cbe2 50
lcorralesc1 0:8e9ddfd2cbe2 51 led3 =1;
lcorralesc1 0:8e9ddfd2cbe2 52 if (button3.falling()) { //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
lcorralesc1 0:8e9ddfd2cbe2 53 led3 =!led3;
lcorralesc1 0:8e9ddfd2cbe2 54 ++j;
lcorralesc1 0:8e9ddfd2cbe2 55 }
lcorralesc1 0:8e9ddfd2cbe2 56
lcorralesc1 0:8e9ddfd2cbe2 57 if (j==0){
lcorralesc1 0:8e9ddfd2cbe2 58 sp=sp+wheel.getPulses();
lcorralesc1 0:8e9ddfd2cbe2 59 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 60 if (sp>999){
lcorralesc1 0:8e9ddfd2cbe2 61 sp=999;
lcorralesc1 0:8e9ddfd2cbe2 62 }
lcorralesc1 0:8e9ddfd2cbe2 63 if (sp<0){
lcorralesc1 0:8e9ddfd2cbe2 64 sp=0;
lcorralesc1 0:8e9ddfd2cbe2 65 }
lcorralesc1 0:8e9ddfd2cbe2 66 lcd.locate(2,0);
lcorralesc1 0:8e9ddfd2cbe2 67 lcd.printf(" ",sp);
lcorralesc1 0:8e9ddfd2cbe2 68 lcd.locate(2,0);
lcorralesc1 0:8e9ddfd2cbe2 69 lcd.printf("%d",sp);
lcorralesc1 0:8e9ddfd2cbe2 70 wait(0.2);
lcorralesc1 0:8e9ddfd2cbe2 71
lcorralesc1 0:8e9ddfd2cbe2 72 if(button3.falling()){
lcorralesc1 0:8e9ddfd2cbe2 73 j=1;
lcorralesc1 0:8e9ddfd2cbe2 74 led3=0;
lcorralesc1 0:8e9ddfd2cbe2 75 wait(0.3);
lcorralesc1 0:8e9ddfd2cbe2 76 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 77 }
lcorralesc1 0:8e9ddfd2cbe2 78
lcorralesc1 0:8e9ddfd2cbe2 79 }
lcorralesc1 0:8e9ddfd2cbe2 80
lcorralesc1 0:8e9ddfd2cbe2 81 if (j==1) {
lcorralesc1 0:8e9ddfd2cbe2 82 kp=kp+wheel.getPulses();
lcorralesc1 0:8e9ddfd2cbe2 83 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 84 if (kp>999){
lcorralesc1 0:8e9ddfd2cbe2 85 kp=999;
lcorralesc1 0:8e9ddfd2cbe2 86 }
lcorralesc1 0:8e9ddfd2cbe2 87 if (kp<0){
lcorralesc1 0:8e9ddfd2cbe2 88 kp=0;
lcorralesc1 0:8e9ddfd2cbe2 89 }
lcorralesc1 0:8e9ddfd2cbe2 90 lcd.locate(10,0);
lcorralesc1 0:8e9ddfd2cbe2 91 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 92 lcd.locate(10,0);
lcorralesc1 0:8e9ddfd2cbe2 93 lcd.printf("%d",kp);
lcorralesc1 0:8e9ddfd2cbe2 94 wait(0.2);
lcorralesc1 0:8e9ddfd2cbe2 95
lcorralesc1 0:8e9ddfd2cbe2 96 if(button3.falling()){
lcorralesc1 0:8e9ddfd2cbe2 97 j=2;
lcorralesc1 0:8e9ddfd2cbe2 98 led3=0;
lcorralesc1 0:8e9ddfd2cbe2 99 wait(0.3);
lcorralesc1 0:8e9ddfd2cbe2 100 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 101 }
lcorralesc1 0:8e9ddfd2cbe2 102
lcorralesc1 0:8e9ddfd2cbe2 103 }
lcorralesc1 0:8e9ddfd2cbe2 104
lcorralesc1 0:8e9ddfd2cbe2 105 if (j==2) {
lcorralesc1 0:8e9ddfd2cbe2 106 ki=ki+wheel.getPulses();
lcorralesc1 0:8e9ddfd2cbe2 107 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 108 if (ki>999){
lcorralesc1 0:8e9ddfd2cbe2 109 ki=999;
lcorralesc1 0:8e9ddfd2cbe2 110 }
lcorralesc1 0:8e9ddfd2cbe2 111 if (ki<0){
lcorralesc1 0:8e9ddfd2cbe2 112 ki=0;
lcorralesc1 0:8e9ddfd2cbe2 113 }
lcorralesc1 0:8e9ddfd2cbe2 114 lcd.locate(2,1);
lcorralesc1 0:8e9ddfd2cbe2 115 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 116 lcd.locate(2,1);
lcorralesc1 0:8e9ddfd2cbe2 117 lcd.printf("%d",ki);
lcorralesc1 0:8e9ddfd2cbe2 118 wait(0.2);
lcorralesc1 0:8e9ddfd2cbe2 119
lcorralesc1 0:8e9ddfd2cbe2 120 if(button3.falling()){
lcorralesc1 0:8e9ddfd2cbe2 121 j=3;
lcorralesc1 0:8e9ddfd2cbe2 122 led3=0;
lcorralesc1 0:8e9ddfd2cbe2 123 wait(0.3);
lcorralesc1 0:8e9ddfd2cbe2 124 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 125 }
lcorralesc1 0:8e9ddfd2cbe2 126
lcorralesc1 0:8e9ddfd2cbe2 127 }
lcorralesc1 0:8e9ddfd2cbe2 128
lcorralesc1 0:8e9ddfd2cbe2 129 if (j==3) {
lcorralesc1 0:8e9ddfd2cbe2 130 kd=kd+wheel.getPulses();
lcorralesc1 0:8e9ddfd2cbe2 131 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 132 if (kd>999){
lcorralesc1 0:8e9ddfd2cbe2 133 kd=999;
lcorralesc1 0:8e9ddfd2cbe2 134 }
lcorralesc1 0:8e9ddfd2cbe2 135 if (kd<0){
lcorralesc1 0:8e9ddfd2cbe2 136 kd=0;
lcorralesc1 0:8e9ddfd2cbe2 137 }
lcorralesc1 0:8e9ddfd2cbe2 138 lcd.locate(10,1);
lcorralesc1 0:8e9ddfd2cbe2 139 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 140 lcd.locate(10,1);
lcorralesc1 0:8e9ddfd2cbe2 141 lcd.printf("%d",kd);
lcorralesc1 0:8e9ddfd2cbe2 142 wait(0.2);
lcorralesc1 0:8e9ddfd2cbe2 143
lcorralesc1 0:8e9ddfd2cbe2 144 if(button3.falling()){
lcorralesc1 0:8e9ddfd2cbe2 145 j=0;
lcorralesc1 0:8e9ddfd2cbe2 146 led3=0;
lcorralesc1 0:8e9ddfd2cbe2 147 wait(0.3);
lcorralesc1 0:8e9ddfd2cbe2 148 wheel.reset();
lcorralesc1 0:8e9ddfd2cbe2 149 }
lcorralesc1 0:8e9ddfd2cbe2 150
lcorralesc1 0:8e9ddfd2cbe2 151 }
lcorralesc1 0:8e9ddfd2cbe2 152
lcorralesc1 0:8e9ddfd2cbe2 153 if (j==4) {
lcorralesc1 0:8e9ddfd2cbe2 154 j=0;
lcorralesc1 0:8e9ddfd2cbe2 155 }
lcorralesc1 0:8e9ddfd2cbe2 156
lcorralesc1 0:8e9ddfd2cbe2 157 if (!button4){
lcorralesc1 0:8e9ddfd2cbe2 158 break; //sale del bucle si pisan suiche4
lcorralesc1 0:8e9ddfd2cbe2 159 }
lcorralesc1 0:8e9ddfd2cbe2 160 } //cierro while(1)
lcorralesc1 0:8e9ddfd2cbe2 161 //%---------------------------------------------------------------------
lcorralesc1 0:8e9ddfd2cbe2 162
lcorralesc1 0:8e9ddfd2cbe2 163
lcorralesc1 0:8e9ddfd2cbe2 164 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
lcorralesc1 0:8e9ddfd2cbe2 165 lcd.cls(); //borra la pantalla
lcorralesc1 0:8e9ddfd2cbe2 166 lcd.printf("GUARDAMOS \nVALORES |m|");
lcorralesc1 0:8e9ddfd2cbe2 167 wait(2);
lcorralesc1 0:8e9ddfd2cbe2 168 lcd.cls();
lcorralesc1 0:8e9ddfd2cbe2 169 lcd.printf(" INICIA EL PID");
lcorralesc1 0:8e9ddfd2cbe2 170 wait(2);
lcorralesc1 0:8e9ddfd2cbe2 171 lcd.cls();
lcorralesc1 0:8e9ddfd2cbe2 172
lcorralesc1 0:8e9ddfd2cbe2 173 // se imprimen los parches del control *****************************************
lcorralesc1 0:8e9ddfd2cbe2 174 lcd.locate(0,0);
lcorralesc1 0:8e9ddfd2cbe2 175 lcd.printf("Er%d",err);
lcorralesc1 0:8e9ddfd2cbe2 176 lcd.locate(8,0);
lcorralesc1 0:8e9ddfd2cbe2 177 lcd.printf("Me%d",med2);
lcorralesc1 0:8e9ddfd2cbe2 178 lcd.locate(0,1);
lcorralesc1 0:8e9ddfd2cbe2 179 lcd.printf("Sp%d",sp);
lcorralesc1 0:8e9ddfd2cbe2 180 lcd.locate(8,1);
lcorralesc1 0:8e9ddfd2cbe2 181 lcd.printf("Con%d",pid);
lcorralesc1 0:8e9ddfd2cbe2 182 //wait(3);
lcorralesc1 0:8e9ddfd2cbe2 183
lcorralesc1 0:8e9ddfd2cbe2 184
lcorralesc1 0:8e9ddfd2cbe2 185 // CICLO PRINCIPAL CONTROLADOR PID
lcorralesc1 0:8e9ddfd2cbe2 186
lcorralesc1 0:8e9ddfd2cbe2 187 while(1) {
lcorralesc1 0:8e9ddfd2cbe2 188 //leer puerto analogo y asignar a med
lcorralesc1 0:8e9ddfd2cbe2 189 med=y.read();
lcorralesc1 0:8e9ddfd2cbe2 190 med2=med*100;
lcorralesc1 0:8e9ddfd2cbe2 191 err=(sp-med2);
lcorralesc1 0:8e9ddfd2cbe2 192
lcorralesc1 0:8e9ddfd2cbe2 193 float kpf;
lcorralesc1 0:8e9ddfd2cbe2 194 kpf=kp*0.001;
lcorralesc1 0:8e9ddfd2cbe2 195 ap = kpf*err;
lcorralesc1 0:8e9ddfd2cbe2 196 float kif;
lcorralesc1 0:8e9ddfd2cbe2 197 kif=ki*0.001;
lcorralesc1 0:8e9ddfd2cbe2 198 ai =(kif*err)+ai;
lcorralesc1 0:8e9ddfd2cbe2 199 float kdf;
lcorralesc1 0:8e9ddfd2cbe2 200 kdf=kd*0.0001;
lcorralesc1 0:8e9ddfd2cbe2 201 ad = kdf*(err-err_v);
lcorralesc1 0:8e9ddfd2cbe2 202 err_v=err;
lcorralesc1 0:8e9ddfd2cbe2 203 pid = (ap+ai+ad);
lcorralesc1 0:8e9ddfd2cbe2 204
lcorralesc1 0:8e9ddfd2cbe2 205 // se verifica que pid sea menor o igual la valor maximo *****************
lcorralesc1 0:8e9ddfd2cbe2 206 if (pid > .99999){
lcorralesc1 0:8e9ddfd2cbe2 207 pid=1;
lcorralesc1 0:8e9ddfd2cbe2 208 }
lcorralesc1 0:8e9ddfd2cbe2 209 // se verifica que pid sea positivo **************************************
lcorralesc1 0:8e9ddfd2cbe2 210 if (pid <0){
lcorralesc1 0:8e9ddfd2cbe2 211 pid=0;
lcorralesc1 0:8e9ddfd2cbe2 212 }
lcorralesc1 0:8e9ddfd2cbe2 213 // se verifica que la accion integral no sea muy grande
lcorralesc1 0:8e9ddfd2cbe2 214 if(ai>0.5){
lcorralesc1 0:8e9ddfd2cbe2 215 ai =0.5;
lcorralesc1 0:8e9ddfd2cbe2 216 }
lcorralesc1 0:8e9ddfd2cbe2 217
lcorralesc1 0:8e9ddfd2cbe2 218 u=pid;
lcorralesc1 0:8e9ddfd2cbe2 219
lcorralesc1 0:8e9ddfd2cbe2 220 //****se muestran las variables******************************************
lcorralesc1 0:8e9ddfd2cbe2 221 if(c>600){
lcorralesc1 0:8e9ddfd2cbe2 222 lcd.locate(2,0);
lcorralesc1 0:8e9ddfd2cbe2 223 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 224 lcd.locate(0,0);
lcorralesc1 0:8e9ddfd2cbe2 225 lcd.printf("Er%2.2f",err);
lcorralesc1 0:8e9ddfd2cbe2 226 lcd.locate(10,0);
lcorralesc1 0:8e9ddfd2cbe2 227 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 228 lcd.locate(8,0);
lcorralesc1 0:8e9ddfd2cbe2 229 lcd.printf("Me%d",med2);
lcorralesc1 0:8e9ddfd2cbe2 230 lcd.locate(2,1);
lcorralesc1 0:8e9ddfd2cbe2 231 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 232 lcd.locate(0,1);
lcorralesc1 0:8e9ddfd2cbe2 233 lcd.printf("Sp%d",sp);
lcorralesc1 0:8e9ddfd2cbe2 234 lcd.locate(10,1);
lcorralesc1 0:8e9ddfd2cbe2 235 lcd.printf(" ");
lcorralesc1 0:8e9ddfd2cbe2 236 lcd.locate(8,1);
lcorralesc1 0:8e9ddfd2cbe2 237 lcd.printf("Con%4.3f",pid);
lcorralesc1 0:8e9ddfd2cbe2 238 c=0;
lcorralesc1 0:8e9ddfd2cbe2 239 }
lcorralesc1 0:8e9ddfd2cbe2 240 else
lcorralesc1 0:8e9ddfd2cbe2 241 c++;
lcorralesc1 0:8e9ddfd2cbe2 242
lcorralesc1 0:8e9ddfd2cbe2 243 }
lcorralesc1 0:8e9ddfd2cbe2 244
lcorralesc1 0:8e9ddfd2cbe2 245
lcorralesc1 0:8e9ddfd2cbe2 246
lcorralesc1 0:8e9ddfd2cbe2 247 }