Alejandro Marin
En este programa se presenta un PID modificando los valores a traves de un encoder
Dependencies: QEI TextLCD mbed
Revision 0:fb37cd782170, committed 2013-11-13
- Comitter:
- amarincan
- Date:
- Wed Nov 13 02:59:07 2013 +0000
- Commit message:
- PID con encoder
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/DebouncedIn.cpp Wed Nov 13 02:59:07 2013 +0000
@@ -0,0 +1,91 @@
+#include "DebouncedIn.h"
+#include "mbed.h"
+
+/*
+ * Constructor
+ */
+DebouncedIn::DebouncedIn(PinName in)
+ : _in(in) {
+
+ // reset all the flags and counters
+ _samples = 0;
+ _output = 0;
+ _output_last = 0;
+ _rising_flag = 0;
+ _falling_flag = 0;
+ _state_counter = 0;
+
+ // Attach ticker
+ _ticker.attach(this, &DebouncedIn::_sample, 0.005);
+}
+
+void DebouncedIn::_sample() {
+
+ // take a sample
+ _samples = _samples >> 1; // shift left
+
+ if (_in) {
+ _samples |= 0x80;
+ }
+
+ // examine the sample window, look for steady state
+ if (_samples == 0x00) {
+ _output = 0;
+ }
+ else if (_samples == 0xFF) {
+ _output = 1;
+ }
+
+
+ // Rising edge detection
+ if ((_output == 1) && (_output_last == 0)) {
+ _rising_flag++;
+ _state_counter = 0;
+ }
+
+ // Falling edge detection
+ else if ((_output == 0) && (_output_last == 1)) {
+ _falling_flag++;
+ _state_counter = 0;
+ }
+
+ // steady state
+ else {
+ _state_counter++;
+ }
+
+ // update the output
+ _output_last = _output;
+
+}
+
+
+
+// return number of rising edges
+int DebouncedIn::rising(void) {
+ int return_value = _rising_flag;
+ _rising_flag = 0;
+ return(return_value);
+}
+
+// return number of falling edges
+int DebouncedIn::falling(void) {
+ int return_value = _falling_flag;
+ _falling_flag = 0;
+ return(return_value);
+}
+
+// return number of ticsk we've bene steady for
+int DebouncedIn::steady(void) {
+return(_state_counter);
+}
+
+// return the debounced status
+int DebouncedIn::read(void) {
+ return(_output);
+}
+
+// shorthand for read()
+DebouncedIn::operator int() {
+ return read();
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/DebouncedIn.h Wed Nov 13 02:59:07 2013 +0000
@@ -0,0 +1,32 @@
+#include "mbed.h"
+
+ class DebouncedIn {
+ public:
+ DebouncedIn(PinName in);
+
+ int read (void);
+ operator int();
+
+ int rising(void);
+ int falling(void);
+ int steady(void);
+
+ private :
+ // objects
+ DigitalIn _in;
+ Ticker _ticker;
+
+ // function to take a sample, and update flags
+ void _sample(void);
+
+ // counters and flags
+ int _samples;
+ int _output;
+ int _output_last;
+ int _rising_flag;
+ int _falling_flag;
+ int _state_counter;
+
+ };
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Nov 13 02:59:07 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Wed Nov 13 02:59:07 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/amarincan/code/TextLCD/#d692719a4c59
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 13 02:59:07 2013 +0000
@@ -0,0 +1,216 @@
+#include "mbed.h"
+#include "DebouncedIn.h"
+#include "TextLCD.h"
+#include "QEI.h"
+
+AnalogIn Vin(PTC2);
+AnalogOut Vout(PTE30);
+
+QEI wheel (PTA16, PTA17, NC, 24);
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+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
+int C2=0x18; // desplaza izquierda
+int C3=0x1A; // desplaza derecha
+int C4=0x0C; // quito cursor bajo
+int i; // indice de la variable
+int j;
+int Kp, Ki, Kd, Sp, yr, cycle, med2 , c,v,m;
+float med,pid,ap, err, ai, ad,err_v;
+int main() {
+
+ lcd.writeCommand(0x0E);
+ lcd.printf("Sp=0 Kp=0");
+ lcd.locate(0,1);
+ lcd.printf("Ki=0 Kd=0");
+ lcd.locate(2,0);
+ lcd.printf("=");
+
+ while(1) {
+
+ m=wheel.getPulses();
+ if(m<0){
+ wheel.reset();
+ m=0;
+ }
+ if(m!=v) {
+
+ switch(i) {
+
+ case 0:
+
+ lcd.locate(2,0);
+ lcd.printf("= ");
+ lcd.locate(3,0);
+ lcd.printf("%d",m);
+ Sp=m;
+
+ break;
+ case 1:
+
+ lcd.locate(10,0);
+ lcd.printf("= ");
+ lcd.locate(11,0);
+ lcd.printf("%d",m);
+ Kp=m;
+ break;
+ case 2:
+ lcd.locate(2,1);
+ lcd.printf("= ");
+ lcd.locate(3,1);
+ lcd.printf("%d",m);
+ Ki=m;
+ break;
+ case 3:
+ lcd.locate(10,1);
+ lcd.printf("= ");
+ lcd.locate(11,1);
+ lcd.printf("%d",m);
+ Kd=m;
+ break;
+ }
+ v=m;
+
+ }
+
+
+ if(button3.falling()) {
+ wheel.reset();
+ i++;
+ if(i>3) {
+ i=0;
+ }
+ switch (i) {
+ case 0:
+ lcd.locate(2,0);
+ lcd.printf("=");
+ break;
+ case 1:
+ lcd.locate(10,0);
+ lcd.printf("=");
+ break;
+ case 2:
+ lcd.locate(2,1);
+ lcd.printf("=");
+ break;
+ case 3:
+ lcd.locate(10,1);
+ lcd.printf("=");
+ break;
+ }
+ }
+
+
+ if (button4.falling()){
+ break; //sale del bucle si pisan suiche4
+ }
+
+ }
+ lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
+ lcd.cls(); //borra la pantalla
+ 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.locate(8,0);
+ lcd.printf("Me%d",med2);
+ lcd.locate(0,1);
+ lcd.printf("Sp%d",Sp);
+ lcd.locate(8,1);
+ lcd.printf("Pid%d",pid);
+ //wait(5);
+
+ // CICLO PRINCIPAL CONTROLADOR PID
+
+ while(1) {
+ //leer puerto analogo y asignar a med
+ med=Vin.read();
+ med2=med*100;
+ err = (Sp-med2);
+ float kp2;
+ kp2=Kp*0.001;
+ ap = kp2*err;
+ float ki2;
+ ki2=Ki*0.001;
+ ai =(ki2*err)+ai; //calculo de la integral del error
+ float kd2;
+ kd2=Kd*0.0001;
+ ad = kd2*(err-err_v); //calculo de la accion derivativa
+ err_v=err; //guarda el error
+ pid = (ap+ai+ad);
+
+ // se verifica que pid sea menor o igual la valor maximo *****************
+ if (pid > .99999){
+ pid=1;
+ }
+
+ // se verifica que pid sea positivo **************************************
+ if (pid <0){
+ pid=0;
+ }
+
+ //wait(.5);
+
+
+
+ // se verifica que la accion integral no sea muy grande
+ if (ai > 999){
+ ai=1000;
+ }
+
+
+ Vout=pid;
+
+
+ //****se muestran las variables******************************************
+ if(c>600){
+ lcd.locate(2,0);
+ lcd.printf(" ");
+ lcd.locate(0,0);
+ lcd.printf("Er%2.2f",err);
+ lcd.locate(10,0);
+ lcd.printf(" ");
+ lcd.locate(8,0);
+ lcd.printf("Me%d",med2);
+ lcd.locate(2,1);
+ lcd.printf(" ");
+ lcd.locate(0,1);
+ lcd.printf("Sp%d",Sp);
+ lcd.locate(10,1);
+ lcd.printf(" ");
+ lcd.locate(8,1);
+ lcd.printf("Pid%4.3f",pid);
+ c=0;
+ }
+ else
+ c++;
+
+
+
+
+
+
+
+ // se envia el valor pid a puerto analogico de salida (D/A) **************
+ // se repite el ciclo
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 13 02:59:07 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file