Santiago Gil
PID_TECLAS
Dependencies: DebouncedIn TextLCD mbed
Fork of
PID_Encoder
by 
Gustavo Ramirez
Diff: main.cpp
- Revision:
- 5:a582c64fce81
- Parent:
- 4:d42fe3777735
--- a/main.cpp Sat May 24 15:05:40 2014 +0000
+++ b/main.cpp Fri Apr 10 02:37:11 2015 +0000
@@ -1,248 +1,347 @@
#include "mbed.h"
#include "DebouncedIn.h"
#include "TextLCD.h"
-#include "QEI.h"
-
+AnalogIn Vin(PTC2);
+AnalogOut u(PTE30);
TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
-QEI wheel (PTD5, PTD0, NC, 100);
-AnalogIn y(PTB1);
-AnalogOut u(PTE30);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
-DigitalOut led3(LED3);
DebouncedIn button1(PTC12);
DebouncedIn button2(PTC13);
DebouncedIn button3(PTC16);
DebouncedIn button4(PTC17);
-
- //codigos movimiento del curzor
- //18 para izquierda
- //1A para derecha
-
-//int C1=0x0E; // solo muestra el curzor
+Timer t; //se define la función del timer interno que ofrece la plataforma mbed
+
+/* codigos movimiento del curzor
+ 18 para izquierda
+ 1A para derecha
+*/
+
+
+int C1=0x0E; // solo muestra el curzor
int C2=0x18; // desplaza izquierda
int C3=0x1A; // desplaza derecha
int C4=0x0C; // quito cursor bajo
-int C1=0x0F;
-
int i; // indice de la variable
-int j; //variable controla cambio 4 posiciones
-int kp, ki, kd, sp;
-float yr, ap, ai, ad, err, med, err_v, cycle, pid;
-int b=0;
-float pidn;
-Timer t;
-
+int j;
+int kp,ki,kd,sp;
+float med,pid,pidn,ap,ai,ad,err,err_v,err_v2;
+int count;
+int esc=1;
int main() {
lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Sp=%d",sp);
+ lcd.locate(6,0);
+ lcd.printf("Kp=%d",kp);
+ lcd.locate(0,1);
+ lcd.printf("Ki=%d",ki);
+ lcd.locate(6,1);
+ lcd.printf("Kd=%d",kd);
+ lcd.locate(12,1);
+ lcd.printf("E%d",esc);
lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
lcd.locate(0,0);
- lcd.printf("Sp%d",sp);
- lcd.locate(8,0);
- lcd.printf("Kp%d",kp);
- lcd.locate(0,1);
- lcd.printf("Ki%d",ki);
- lcd.locate(8,1);
- lcd.printf("Kd%d",kd);
-
+ lcd.printf("Sp=%d",sp);
+
while(1) {
-
- led3 =1;
- if (button3.falling()) { //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
- led3 =!led3;
+ if (button3.falling()) {
++j;
- }
-
- if (j==0){
- sp=sp+wheel.getPulses();
- wheel.reset();
- if (sp>999){
- sp=999;
- }
- if (sp<0){
- sp=0;
- }
- lcd.locate(2,0);
- lcd.printf(" ",sp);
- lcd.locate(2,0);
+ } //INCREMENTA POSICION DEL MENU CON BOTON 3
+ if (j==0){
+ lcd.locate(3,0);
+ lcd.printf("%d",sp);
+
+ if (button1.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button1==0){
+
+
+ if (sp<999){
+ if (t<=2){++sp;}
+ wait(0.2);
+ if (t>2&&t<=5){sp=sp+5;}
+ wait(0.2);
+ if (t>5&&t<=10){sp=sp+10;}
+ wait(0.2);
+ if (t>10){sp=sp+100;}
+ wait(0.2);
+ if (sp>999){sp=999;}
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
lcd.printf("%d",sp);
- wait(0.2);
+
+ }
+ }
+ }
+ if (button2.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button2==0)
+
+ if (sp>0){
+ if (t<=2){--sp;}
+ wait(0.2);
+ if (t>2&&t<=5){sp=sp-5;}
+ wait(0.2);
+ if (t>5&&t<=10){sp=sp-10;}
+ wait(0.2);
+ if (t>10){sp=sp-100;}
+ wait(0.2);
+ if (sp<0){sp=0;}
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%d",sp);
+ }
+ }
+ }
+ if (j==1) {
- if(button3.falling()){
- j=1;
- led3=0;
- wait(0.3);
- wheel.reset();
- }
-
- }
-
- if (j==1) {
- kp=kp+wheel.getPulses();
- wheel.reset();
- if (kp>999){
- kp=999;
- }
- if (kp<0){
- kp=0;
- }
- lcd.locate(10,0);
- lcd.printf(" ");
- lcd.locate(10,0);
+ if (button1.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button1==0){
+
+ if (kp<1000){
+ if (t<=2){++kp;}
+ wait(0.2);
+ if (t>2&&t<=5){kp=kp+5;}
+ wait(0.2);
+ if (t>5&&t<=10){kp=kp+10;}
+ wait(0.2);
+ if (t>10){kp=kp+100;}
+ wait(0.2);
+ if (kp>999){kp=999;}
+ lcd.locate(9,0);
+ lcd.printf(" ");
+ lcd.locate(9,0);
lcd.printf("%d",kp);
- wait(0.2);
-
- if(button3.falling()){
- j=2;
- led3=0;
- wait(0.3);
- wheel.reset();
- }
-
- }
-
+
+ }
+ }
+ }
+ if (button2.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button2==0){
+ if (kp>=0){
+ if (t<=2){--kp;}
+ wait(0.2);
+ if (t>2&&t<=5){kp=kp-5;}
+ wait(0.2);
+ if (t>5&&t<=10){kp=kp-10;}
+ wait(0.2);
+ if (t>10){kp=kp-100;}
+ wait(0.2);
+ if(kp<0){kp=0;}
+ lcd.locate(9,0);
+ lcd.printf(" ");
+ lcd.locate(9,0);
+ lcd.printf("%d",kp);
+ }
+ }
+ }
+ }
if (j==2) {
- ki=ki+wheel.getPulses();
- wheel.reset();
- if (ki>999){
- ki=999;
- }
- if (ki<0){
- ki=0;
- }
- lcd.locate(2,1);
- lcd.printf(" ");
- lcd.locate(2,1);
+ if (button1.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button1==0){
+
+
+ if (ki<1000){
+ if (t<=2){++ki;}
+ wait(0.2);
+ if (t>2&&t<=5){ki=ki+5;}
+ wait(0.2);
+ if (t>5&&t<=10){ki=ki+10;}
+ wait(0.2);
+ if (t>10){ki=ki+100;}
+ wait(0.2);
+ if (ki>999){ki=999;}
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
lcd.printf("%d",ki);
- wait(0.2);
-
- if(button3.falling()){
- j=3;
- led3=0;
- wait(0.3);
- wheel.reset();
- }
-
- }
-
+
+ }
+ }
+ }
+ if (button2.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button2==0){
+
+ if (ki>=0){
+ if (t<=2){--ki;}
+ wait(0.2);
+ if (t>2&&t<=5){ki=ki-5;}
+ wait(0.2);
+ if (t>5&&t<=10){ki=ki-10;}
+ wait(0.2);
+ if (t>10){ki=ki-100;}
+ wait(0.2);
+ if (ki<0){ki=0;}
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d",ki);
+
+ }
+ }
+ }
+ }
+
if (j==3) {
- kd=kd+wheel.getPulses();
- wheel.reset();
- if (kd>999){
- kd=999;
- }
- if (kd<0){
- kd=0;
- }
- lcd.locate(10,1);
- lcd.printf(" ");
- lcd.locate(10,1);
+ if (button1.falling()) {
+ t.stop();
+ t.reset();
+ t.start();
+ while (button1==0){
+
+
+ if (kd<1000){
+ if (t<=2){++kd;}
+ wait(0.2);
+ if (t>2&&t<=5){kd=kd+5;}
+ wait(0.2);
+ if (t>5&&t<=10){kd=kd+10;}
+ wait(0.2);
+ if (t>10){kd=kd+100;}
+ wait(0.2);
+ if (kd>999){kd=999;}
+ lcd.locate(9,1);
+ lcd.printf(" ");
+ lcd.locate(9,1);
lcd.printf("%d",kd);
- wait(0.2);
+
+ }
+ }
+ }
+ if (button2.falling()) {
+
+ t.stop();
+ t.reset();
+ t.start();
+ while (button2==0){
+ if (kd>=0){
+ if (t<=2){--kd;}
+ wait(0.2);
+
+ if (t>2&&t<=5){kd=kd-5;}
+
+ wait(0.2);
+ if (t>5&&t<=10){kd=kd-10;}
+ wait(0.2);
+ if (t>10){kd=kd-100;}
+ wait(0.2);
+ if (kd<0){kd=0;}
+ lcd.locate(9,1);
+ lcd.printf(" ");
+ lcd.locate(9,1);
+ lcd.printf("%d",kd);
+ }
+ }
+ }
+ }
+ if (j==4) {
- if(button3.falling()){
- j=0;
- led3=0;
- wait(0.3);
- wheel.reset();
- }
-
- }
-
- if (j==4) {
+ if (button1.falling()){
+ if (esc<1000){
+ esc=esc*10;
+ lcd.locate(13,1);
+ lcd.printf(" ");
+ lcd.locate(13,1);
+ lcd.printf("%d",esc);
+ }
+ }
+ if (button2.falling()){
+ if (esc>1){
+ esc=esc/10;
+ lcd.locate(13,1);
+ lcd.printf(" ");
+ lcd.locate(13,1);
+ lcd.printf("%d",esc);
+ }
+ }
+ }
+ if (j==5) {
j=0;
- }
-
- if (!button4){
- break; //sale del bucle si pisan suiche4
- }
- } //cierro while(1)
-//%---------------------------------------------------------------------
-
-
- lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
+ }
+ if (button4.falling()){
+ break; //sale del bucle si pisan suiche4
+ }
+
+ }
+
+ lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
lcd.cls(); //borra la pantalla
- lcd.printf("GUARDAMOS \nVALORES |m|");
+ lcd.printf(" GUARDADOS!");
wait(2);
-
+ lcd.cls();
+ lcd.printf(" INICIA EL PID");
+ wait(2);
// se imprimen los parches del control *****************************************
- lcd.cls();
- lcd.printf("Er%d",err);
+ lcd.cls();
+ lcd.printf("Er%01.3f",err);
lcd.locate(8,0);
- lcd.printf("Me%d",med);
+ lcd.printf("Me%01.3f",med);
lcd.locate(0,1);
lcd.printf("Sp%d",sp);
lcd.locate(8,1);
- lcd.printf("Co%d",pid);
- wait(1);
-
+ lcd.printf("Co%01.3f",pid);
+ wait(5);
// CICLO PRINCIPAL CONTROLADOR PID
- b=0;
+
while(1) {
- med=1000*y.read(); //leer puerto analogo y asignar a med
+ med=Vin.read()/esc; //leer puerto analogo y asignar a med
err = (sp-med);
- ap = kp*err;
-
- // se verifica que la accion integral no sea muy grande
+ ap = kp*(err-err_v2);
if(ai<100)
{
- ai =(ki*err)+ai; //calculo de la integral del error
+ ai =(ki*(err-err_v))+ai; //calculo de la integral del error
}
- //else{
- //Dejo de sumar la accion integral
- // }
-
- ad = kd*(err-err_v); //calculo de la accion derivativa
+ // se verifica que la accion integral no sea muy grande
+ ad = kd*(err-(2*err_v)+err_v2); //calculo de la accion derivativa
pid = (ap+ai+ad);
-
-
-
- // se verifica que pid sea positivo **************************************
- if(pid<=0){pid=0;}
- // se verifica que pid sea menor o igual la valor maximo *****************
- if(pid>=1000){pid=1000;}
-
- // se actualizan las variables *******************************************
- err_v = err;
-
- //se muestran las variables******************************************
-
- if (b==0)
- {
- t.start();
- b=1;
- }
- if(t>=0.01)
- {
- wait(0.2);
+ //****se muestran las variables******************************************
+ if (pid > 999){
+ pid=1000;
+ }
+ if (pid < 0){
+ pid=0;
+ }
+ wait(.5);
lcd.locate(2,0);
- lcd.printf(" ");
- lcd.locate(2,0);
- lcd.printf("%d",err);
- lcd.locate(10,0);
- lcd.printf(" ");
+ lcd.printf("%01.3f",err);
lcd.locate(10,0);
- lcd.printf("%d",med);
- lcd.locate(2,1);
- lcd.printf(" ");
+ lcd.printf("%01.3f",med);
lcd.locate(2,1);
- lcd.printf("%d",sp);
- lcd.locate(10,1);
- lcd.printf(" ");
+ lcd.printf("%01.3f",sp);
lcd.locate(10,1);
- lcd.printf("%d",pid);
- }
+ lcd.printf("%01.3f",pid);
+
- //Normalizacion de la salida
- pidn=pid/1000;
- // se envia el valor pid a puerto analogico de salida (D/A) **************
+ // se actualizan las variables *******************************************
+ err_v2=err_v;
+ err_v = err;
+ pidn=pid/esc;
+ // se verifica que pid sea positivo **************************************
u.write(pidn);
-
+ // se verifica que pid sea menor o igual la valor maximo *****************
+
+
+ // se envia el valor pid a puerto analogico de salida (D/A) **************
// se repite el ciclo
- wait(0.1);
}
-
-
- }
-
\ No newline at end of file
+}
\ No newline at end of file