deiwid ricaurte
controlpid por control remoto
Dependencies: Pulse1 QEI TextLCD mbed
Fork of
irda1
by 
Gustavo Ramirez
Diff: main.cpp
- Revision:
- 3:786400a8fd7c
- Parent:
- 2:6a15ab0305c8
--- a/main.cpp Fri Nov 08 07:40:04 2013 +0000
+++ b/main.cpp Thu Nov 24 01:06:46 2016 +0000
@@ -1,6 +1,12 @@
#include "mbed.h"
#include <Pulse1.h>
+#include "QEI.h"
+#include "TextLCD.h"
+
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+AnalogIn y(PTB2);//entrada analoga
+AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
PulseInOut irda(PTD5);// en este puerto se pone el sensor infrarrojo
Serial pc(USBTX, USBRX);
DigitalOut led(LED1);
@@ -13,36 +19,405 @@
const int T_bajo=526;//ponga su tiempo de la prueba
const int num_bits = 32;//ponga su numero de bits
int num[num_bits];//cadena para almacenar todos los tiempos que conforman los bits de datos
+int num1[num_bits];
+int arriba[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,1,1,0,1,0,0,0,0,0,0,1,0,1,1,1};
+int abajo[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,1,1,0,0,0,0,1,1,0,0,1,1,1};
+int derecha[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,1,1,1,0,0,0,1,1,0,0,0,1,1};
+int ok[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,1,0,1,0,1,0,0,0,0,1,0,1,0,1,1};
int dato; // tiempo de cada dato que se lee
-
+int a=0;
+int b=0;
+int c=0;
+int d=0;
+
+
+int C2=0x18; // desplaza izquierda
+int C3=0x1A; // desplaza derecha
+int C4=0x0C; // quito cursor bajo
+
+int C1=0x0F;
+int cambio=0, diferencia=0;
+float pid,o,ai,ad,ap,med,err;
+float err_v;
+int sp=0,ki=0,kp=0,kd=0,pos=1;
+
+
int main(){
+lcd.locate(0,1);
+lcd.printf("**Control PID**");
+wait(2);
+lcd.cls(); // Borrar Pantalla
+lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
+
+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.locate(0,0);
+lcd.printf("Sp=%d",sp);
+
while(1){
-ini1: header=0;
+ ini1: header=0;
+ a=0;
+ b=0;
+ c=0;
+ d=0;
led2=1;
led=1;
header = irda.read_low_us(); //funcion para leer un pulso de caida o bajo
if (header > head_L && header < head_H) goto seguir;//verificar que este en la tolerancia +-20%
else goto ini1;
-seguir:
+ seguir:
//leo los datos de la trama y se meten a un arreglo
wait_us(5000);
led2=0;
- for(i=0;i<(num_bits-1);++i){ // POR OSCILOSCOPIO se determina que llegan (num_bits),datos
- dato = irda.read_high_us(); //leer un bit de datos que es pulso arriba en este control
- num[i]=dato;
- wait_us(300);
- }
+ for(i=0;i<(num_bits-1);++i)
+ { // POR OSCILOSCOPIO se determina que llegan (num_bits),datos
+ dato = irda.read_high_us(); //leer un bit de datos que es pulso arriba en este control
+ num[i]=dato;
+ wait_us(300);
+ }
wait(0.5); //espero un poquito antes de leer todo el arreglo y ponerlo en pantalla
pc.printf(",%d",header);
- for(i=0;i<(num_bits-1);++i){
- pc.printf(",%d",num[i]);
- }
+ for(i=0;i<(num_bits-1);++i)
+ {
+ pc.printf(",%d",num[i]);
+ }
wait(0.1); //espero e imprimo en binario
pc.printf("\n\n");
- for(i=0;i<(num_bits-1);++i){
- if(num[i] > ((T_alto+T_bajo)/2)) pc.printf("1");
- else pc.printf("0");
- }
- }
- }
\ No newline at end of file
+ for(i=0;i<(num_bits-1);++i)
+ {
+
+ if(num[i] > ((T_alto+T_bajo)/2))
+ {
+ pc.printf("1");
+ num1[i]=1;
+ if(num1[i]==arriba[i]){
+ a=a+1;
+
+ }
+ if(num1[i]==abajo[i]){
+ b=b+1;
+
+ }
+ if(num1[i]==derecha[i]){
+ c=c+1;
+
+ }
+ if(num1[i]==ok[i]){
+ d=d+1;
+
+ }
+ }
+ else
+ {
+ pc.printf("0");
+ num1[i]=0;
+ if(num1[i]==arriba[i]){
+ a=a+1;
+ }
+ if(num1[i]==abajo[i]){
+ b=b+1;
+ }
+ if(num1[i]==derecha[i]){
+ c=c+1;
+ }
+ if(num1[i]==ok[i]){
+ d=d+1;
+ }
+ }
+ }
+ if(a==31)
+ {
+ a=0;
+ b=0;
+ c=0;
+ d=0;
+ if(pos==1)
+ {
+
+ if(sp+1>=999)
+ {
+ sp=999;
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ }
+ else
+ {
+ sp+=1;
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+
+ }
+ }
+ else if(pos==2)
+ {
+
+ if(kp+1>=999)
+ {
+ kp=999;
+ lcd.locate(11,0);
+ lcd.printf(" ");
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ }
+ else
+ {
+ kp+=1;
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+
+ }
+ }
+ else if(pos==3)
+ {
+
+ if(ki+1>=999)
+ {
+ sp=999;
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ }
+ else
+ {
+ ki+=1;
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+
+ }
+ }
+ else if(pos==4)
+ {
+
+ if(kd+1>=999)
+ {
+ sp=999;
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ }
+ else
+ {
+ kd+=1;
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+
+ }
+ }
+ }
+
+ else if(b==31)
+ {
+ a=0;
+ b=0;
+ c=0;
+ d=0;
+ if(pos==1)
+ {
+
+ if(sp-1<=0)
+ {
+ sp=0;
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ }
+ else
+ {
+ sp-=1;
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+
+ }
+ }
+ else if(pos==2)
+ {
+
+ if(kp-1<=0)
+ {
+ kp=0;
+ lcd.locate(11,0);
+ lcd.printf(" ");
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ }
+ else
+ {
+ kp-=1;
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+
+ }
+ }
+ else if(pos==3)
+ {
+
+ if(ki-1<=0)
+ {
+ sp=0;
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ }
+ else
+ {
+ ki-=1;
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+
+ }
+ }
+ else if(pos==4)
+ {
+
+ if(kd-1<=0)
+ {
+ sp=0;
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ }
+ else
+ {
+ kd-=1;
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+
+ }
+ }
+ }
+
+ if(c==31)
+ {
+ a=0;
+ b=0;
+ c=0;
+ d=0;
+ led2 =!led2;
+ if(pos==4)
+ {
+ pos=1;
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ }
+ else if (pos==1)
+ {
+ pos++;
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ }
+ else if(pos==2)
+ {
+ pos++;
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ }
+ else if(pos==3)
+ {
+ pos++;
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ }
+ wait(0.25);
+ }
+
+ if(d==31)
+ {
+ a=0;
+ b=0;
+ c=0;
+ d=0;
+ break; //sale del bucle si pulsan ok
+ }
+ else
+ {
+ a=0;
+ b=0;
+ c=0;
+ d=0;
+ }
+ goto ini1;
+
+
+ }
+//Transicion
+ lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
+ lcd.cls(); //borra la pantalla
+ lcd.printf(" GUARDADOS!");
+ wait(1);
+ lcd.cls();
+ lcd.printf(" INICIA EL PID");
+ wait(1);
+// se imprimen los parches del control *****************************************
+ lcd.cls();
+ lcd.printf("Er=%3.0f",err);
+ lcd.locate(8,0);
+ lcd.printf("Me=%3.0f",med);
+ lcd.locate(0,1);
+ lcd.printf("Sp=%3.0f",sp);
+ lcd.locate(8,1);
+ lcd.printf("Co=%3.0f",pid);
+ wait(1);
+
+// CICLO PRINCIPAL CONTROLADOR PID
+ lop1: med = y.read()*999;
+ err = (sp-med); //se calcula el error
+ ap = kp*err*0.01f; //se calcula la accion proporcinal
+ ai =(ki*err*0.01f)+ai; //calculo de la integral del error
+ ad = kd*(err-err_v)*0.01f; //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 > 999)
+ {
+ pid=999;
+ }
+
+
+ //se muestran las variables******************************************
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%3.0f",err);
+ lcd.locate(11,0);
+ lcd.printf(" ");
+ lcd.locate(11,0);
+ lcd.printf("%3.0f",med);
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d",sp);
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%3.0f",pid);
+
+
+
+
+ //Normalizacion de la salida
+ // se actualizan las variables *******************************************
+ err_v = err;
+
+ o = pid/999;
+ u.write(o);
+
+ wait_ms(300);
+ goto lop1;
+}
\ No newline at end of file