German Camilo Diaz Mejia / Mbed 2 deprecated PID_ENCODER

Programa para colocar las constantes de un PID con encoder y controlar una planta

Dependencies:   DebounceIn TextLCD mbed

main.cpp@0:65c8fa8a589b, 2015-10-23 (annotated)

Committer:
GermanD
Date:
Fri Oct 23 20:45:49 2015 +0000
Revision:
0:65c8fa8a589b
Programa para colocar las constantes de un controlador PID con un encoder y hacer el control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GermanD 0:65c8fa8a589b 1 #include "mbed.h"
GermanD 0:65c8fa8a589b 2 #include "DebouncedIn.h"
GermanD 0:65c8fa8a589b 3 #include "TextLCD.h"
GermanD 0:65c8fa8a589b 4 #include "QEI.h"
GermanD 0:65c8fa8a589b 5
GermanD 0:65c8fa8a589b 6 AnalogIn Vin(PTC2); // entrada analoga
GermanD 0:65c8fa8a589b 7 AnalogOut Vout(PTE30); // salida analoga
GermanD 0:65c8fa8a589b 8 //voltaje de salida maximo= 3.3 V
GermanD 0:65c8fa8a589b 9
GermanD 0:65c8fa8a589b 10 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
GermanD 0:65c8fa8a589b 11
GermanD 0:65c8fa8a589b 12 QEI wheel (PTD5, PTD0, NC, 150);
GermanD 0:65c8fa8a589b 13 DigitalOut led1(LED1);
GermanD 0:65c8fa8a589b 14 DigitalOut led2(LED2);
GermanD 0:65c8fa8a589b 15 DigitalOut led3(LED3);
GermanD 0:65c8fa8a589b 16 DebouncedIn botonEncoder(PTC5);
GermanD 0:65c8fa8a589b 17 DebouncedIn button4(PTC17); // pulsador par dar "enter"
GermanD 0:65c8fa8a589b 18
GermanD 0:65c8fa8a589b 19
GermanD 0:65c8fa8a589b 20 //int C1=0x0E; // muestra el cursor
GermanD 0:65c8fa8a589b 21 int C2=0x18; // desplaza izquierda
GermanD 0:65c8fa8a589b 22 int C3=0x1A; // desplaza derecha
GermanD 0:65c8fa8a589b 23 int C4=0x0C; // quito cursor bajo
GermanD 0:65c8fa8a589b 24 int C1=0x0F;
GermanD 0:65c8fa8a589b 25 int sp=0,kp=0,kd=0,ki=0,p=1,bandera = 0;
GermanD 0:65c8fa8a589b 26 int i; // indice de la variable
GermanD 0:65c8fa8a589b 27 int j; //variable controla cambio 4 posiciones
GermanD 0:65c8fa8a589b 28 float err;
GermanD 0:65c8fa8a589b 29 float pwmset;
GermanD 0:65c8fa8a589b 30 float eInteg;
GermanD 0:65c8fa8a589b 31 float pGain;
GermanD 0:65c8fa8a589b 32 float ePrev ;
GermanD 0:65c8fa8a589b 33 float iGain;
GermanD 0:65c8fa8a589b 34 float dGain;
GermanD 0:65c8fa8a589b 35 float x;
GermanD 0:65c8fa8a589b 36 int main()
GermanD 0:65c8fa8a589b 37 {
GermanD 0:65c8fa8a589b 38 lcd.cls();
GermanD 0:65c8fa8a589b 39 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
GermanD 0:65c8fa8a589b 40 lcd.locate(8,0);
GermanD 0:65c8fa8a589b 41 lcd.printf("kp=%d",kp);
GermanD 0:65c8fa8a589b 42 lcd.locate(0,1);
GermanD 0:65c8fa8a589b 43 lcd.printf("Ki=%d", ki);
GermanD 0:65c8fa8a589b 44 lcd.locate(8,1);
GermanD 0:65c8fa8a589b 45 lcd.printf("Kd=%d", kd);
GermanD 0:65c8fa8a589b 46 lcd.locate(0,0);
GermanD 0:65c8fa8a589b 47 lcd.printf("Sp=%d", sp);
GermanD 0:65c8fa8a589b 48
GermanD 0:65c8fa8a589b 49 while(1) {
GermanD 0:65c8fa8a589b 50
GermanD 0:65c8fa8a589b 51 led3 =1;
GermanD 0:65c8fa8a589b 52 if (botonEncoder.falling()) { //INCREMENTA POSICION DEL MENU CON EL SWITCH DEL ENCODER
GermanD 0:65c8fa8a589b 53 led3 =!led3;
GermanD 0:65c8fa8a589b 54 ++j;
GermanD 0:65c8fa8a589b 55 }
GermanD 0:65c8fa8a589b 56
GermanD 0:65c8fa8a589b 57 if (j==0){
GermanD 0:65c8fa8a589b 58 sp=sp+wheel.getPulses();
GermanD 0:65c8fa8a589b 59 wheel.reset();
GermanD 0:65c8fa8a589b 60 if (sp>5){
GermanD 0:65c8fa8a589b 61 sp=5;
GermanD 0:65c8fa8a589b 62 }
GermanD 0:65c8fa8a589b 63 if (sp<0){
GermanD 0:65c8fa8a589b 64 sp=0;
GermanD 0:65c8fa8a589b 65 }
GermanD 0:65c8fa8a589b 66 lcd.locate(3,0);
GermanD 0:65c8fa8a589b 67 lcd.printf(" ",sp);
GermanD 0:65c8fa8a589b 68 lcd.locate(3,0);
GermanD 0:65c8fa8a589b 69 lcd.printf("%d",sp);
GermanD 0:65c8fa8a589b 70 wait(0.2);
GermanD 0:65c8fa8a589b 71
GermanD 0:65c8fa8a589b 72 if(botonEncoder.falling()){
GermanD 0:65c8fa8a589b 73 j=1;
GermanD 0:65c8fa8a589b 74 led3=0;
GermanD 0:65c8fa8a589b 75 wait(0.3);
GermanD 0:65c8fa8a589b 76 wheel.reset();
GermanD 0:65c8fa8a589b 77 }
GermanD 0:65c8fa8a589b 78
GermanD 0:65c8fa8a589b 79 }
GermanD 0:65c8fa8a589b 80
GermanD 0:65c8fa8a589b 81 if (j==1) {
GermanD 0:65c8fa8a589b 82 kp=kp+wheel.getPulses();
GermanD 0:65c8fa8a589b 83 wheel.reset();
GermanD 0:65c8fa8a589b 84 if (kp>999){
GermanD 0:65c8fa8a589b 85 kp=999;
GermanD 0:65c8fa8a589b 86 }
GermanD 0:65c8fa8a589b 87 if (kp<0){
GermanD 0:65c8fa8a589b 88 kp=0;
GermanD 0:65c8fa8a589b 89 }
GermanD 0:65c8fa8a589b 90 lcd.locate(11,0);
GermanD 0:65c8fa8a589b 91 lcd.printf(" ");
GermanD 0:65c8fa8a589b 92 lcd.locate(11,0);
GermanD 0:65c8fa8a589b 93 lcd.printf("%d",kp);
GermanD 0:65c8fa8a589b 94 wait(0.2);
GermanD 0:65c8fa8a589b 95
GermanD 0:65c8fa8a589b 96 if(botonEncoder.falling()){
GermanD 0:65c8fa8a589b 97 j=2;
GermanD 0:65c8fa8a589b 98 led3=0;
GermanD 0:65c8fa8a589b 99 wait(0.3);
GermanD 0:65c8fa8a589b 100 wheel.reset();
GermanD 0:65c8fa8a589b 101 }
GermanD 0:65c8fa8a589b 102
GermanD 0:65c8fa8a589b 103 }
GermanD 0:65c8fa8a589b 104
GermanD 0:65c8fa8a589b 105 if (j==2) {
GermanD 0:65c8fa8a589b 106 ki=ki+wheel.getPulses();
GermanD 0:65c8fa8a589b 107 wheel.reset();
GermanD 0:65c8fa8a589b 108 if (ki>999){
GermanD 0:65c8fa8a589b 109 ki=999;
GermanD 0:65c8fa8a589b 110 }
GermanD 0:65c8fa8a589b 111 if (ki<0){
GermanD 0:65c8fa8a589b 112 ki=0;
GermanD 0:65c8fa8a589b 113 }
GermanD 0:65c8fa8a589b 114 lcd.locate(3,1);
GermanD 0:65c8fa8a589b 115 lcd.printf(" ");
GermanD 0:65c8fa8a589b 116 lcd.locate(3,1);
GermanD 0:65c8fa8a589b 117 lcd.printf("%d",ki);
GermanD 0:65c8fa8a589b 118 wait(0.2);
GermanD 0:65c8fa8a589b 119
GermanD 0:65c8fa8a589b 120 if(botonEncoder.falling()){
GermanD 0:65c8fa8a589b 121 j=3;
GermanD 0:65c8fa8a589b 122 led3=0;
GermanD 0:65c8fa8a589b 123 wait(0.3);
GermanD 0:65c8fa8a589b 124 wheel.reset();
GermanD 0:65c8fa8a589b 125 }
GermanD 0:65c8fa8a589b 126
GermanD 0:65c8fa8a589b 127 }
GermanD 0:65c8fa8a589b 128
GermanD 0:65c8fa8a589b 129 if (j==3) {
GermanD 0:65c8fa8a589b 130 kd=kd+wheel.getPulses();
GermanD 0:65c8fa8a589b 131 wheel.reset();
GermanD 0:65c8fa8a589b 132 if (kd>999){
GermanD 0:65c8fa8a589b 133 kd=999;
GermanD 0:65c8fa8a589b 134 }
GermanD 0:65c8fa8a589b 135 if (kd<0){
GermanD 0:65c8fa8a589b 136 kd=0;
GermanD 0:65c8fa8a589b 137 }
GermanD 0:65c8fa8a589b 138 lcd.locate(11,1);
GermanD 0:65c8fa8a589b 139 lcd.printf(" ");
GermanD 0:65c8fa8a589b 140 lcd.locate(11,1);
GermanD 0:65c8fa8a589b 141 lcd.printf("%d",kd);
GermanD 0:65c8fa8a589b 142 wait(0.2);
GermanD 0:65c8fa8a589b 143
GermanD 0:65c8fa8a589b 144 if(botonEncoder.falling()){
GermanD 0:65c8fa8a589b 145 j=0;
GermanD 0:65c8fa8a589b 146 led3=0;
GermanD 0:65c8fa8a589b 147 wait(0.3);
GermanD 0:65c8fa8a589b 148 wheel.reset();
GermanD 0:65c8fa8a589b 149 }
GermanD 0:65c8fa8a589b 150
GermanD 0:65c8fa8a589b 151 }
GermanD 0:65c8fa8a589b 152
GermanD 0:65c8fa8a589b 153 if (j==4) {
GermanD 0:65c8fa8a589b 154 j=0;
GermanD 0:65c8fa8a589b 155 }
GermanD 0:65c8fa8a589b 156
GermanD 0:65c8fa8a589b 157 if (!button4){
GermanD 0:65c8fa8a589b 158 break; //sale del bucle si pisan suiche4
GermanD 0:65c8fa8a589b 159 }
GermanD 0:65c8fa8a589b 160 } //cierro while(1)
GermanD 0:65c8fa8a589b 161
GermanD 0:65c8fa8a589b 162 //enter
GermanD 0:65c8fa8a589b 163 if(button4.falling())
GermanD 0:65c8fa8a589b 164 {
GermanD 0:65c8fa8a589b 165 lcd.cls(); // al undir el boton 4 se borra la pantalla
GermanD 0:65c8fa8a589b 166 //for (int h=0;h<100;h++)
GermanD 0:65c8fa8a589b 167 //Vout= Vout+0.01;
GermanD 0:65c8fa8a589b 168 while(bandera==0)
GermanD 0:65c8fa8a589b 169 {
GermanD 0:65c8fa8a589b 170 wait(0.2);
GermanD 0:65c8fa8a589b 171 x=Vout.read(); // lee el voltaje de salida que nuestra analoga pero en porcentaje
GermanD 0:65c8fa8a589b 172
GermanD 0:65c8fa8a589b 173 //}
GermanD 0:65c8fa8a589b 174 //pid
GermanD 0:65c8fa8a589b 175
GermanD 0:65c8fa8a589b 176 err = sp-x; // Calcula el error
GermanD 0:65c8fa8a589b 177 pwmset = kp* err+ki * eInteg +kd* (err - ePrev); // ecuacion para el PID
GermanD 0:65c8fa8a589b 178 pwmset=(0.976801/5)*pwmset; // como el voltaje calculado fue un porcentaje se realiza una conversion respectiva al valor de salida
GermanD 0:65c8fa8a589b 179 Vout=pwmset;
GermanD 0:65c8fa8a589b 180 eInteg =eInteg+ err; // integral
GermanD 0:65c8fa8a589b 181 ePrev = err;
GermanD 0:65c8fa8a589b 182 lcd.locate(0,1);
GermanD 0:65c8fa8a589b 183 lcd.printf("error=%f",err);
GermanD 0:65c8fa8a589b 184 lcd.locate(1,0);
GermanD 0:65c8fa8a589b 185 lcd.printf("salida=%f",x);
GermanD 0:65c8fa8a589b 186 if (button4.falling())
GermanD 0:65c8fa8a589b 187 { bandera=1;
GermanD 0:65c8fa8a589b 188 lcd.cls();
GermanD 0:65c8fa8a589b 189 p=1;
GermanD 0:65c8fa8a589b 190 lcd.locate(8,0);
GermanD 0:65c8fa8a589b 191 lcd.printf("kp=%d",kp);
GermanD 0:65c8fa8a589b 192 lcd.locate(0,1);
GermanD 0:65c8fa8a589b 193 lcd.printf("Ki=%d", ki);
GermanD 0:65c8fa8a589b 194 lcd.locate(8,1);
GermanD 0:65c8fa8a589b 195 lcd.printf("Kd=%d", kd);
GermanD 0:65c8fa8a589b 196 lcd.locate(0,0);
GermanD 0:65c8fa8a589b 197 lcd.printf("Sp=%d", sp);
GermanD 0:65c8fa8a589b 198
GermanD 0:65c8fa8a589b 199 }
GermanD 0:65c8fa8a589b 200
GermanD 0:65c8fa8a589b 201
GermanD 0:65c8fa8a589b 202 }
GermanD 0:65c8fa8a589b 203 bandera = 0;
GermanD 0:65c8fa8a589b 204 }
GermanD 0:65c8fa8a589b 205
GermanD 0:65c8fa8a589b 206
GermanD 0:65c8fa8a589b 207
GermanD 0:65c8fa8a589b 208 }
GermanD 0:65c8fa8a589b 209
GermanD 0:65c8fa8a589b 210
GermanD 0:65c8fa8a589b 211