naoto tanaka
practice for PID control
Dependencies: PID QEI USBDevice mbed
Diff: main.cpp
- Revision:
- 0:b2973a157cb6
- Child:
- 1:6d5c35b995fb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 15 02:02:38 2014 +0000
@@ -0,0 +1,253 @@
+#include "mbed.h"
+#include "USBSerial.h"
+#include "QEI.h"
+#include "PID.h"
+
+#define PIDRATE 0.01
+#define CW 0x01
+#define CCW 0x02
+#define STOP 0x03
+#define FREE 0x00
+
+#define kc 2.8
+#define ti 16.0
+#define td 0.0
+
+USBSerial vcom;
+SPI spi(P0_21, NC, P1_20);
+DigitalOut cs(P1_23);
+
+//Ticker flip;
+
+Timer timer;
+
+PID controller(kc, ti, td, PIDRATE);
+
+QEI wheel (P1_15, P0_19, NC, 1, QEI::X4_ENCODING);
+
+BusOut gled(P0_8, P0_9);
+BusOut mdrv(P1_27, P1_26);
+
+DigitalIn SW(P0_1);
+
+void pidsetup();
+
+union MCP4922
+{
+ uint16_t command;
+ struct
+ {
+ //DAC data bits
+ uint16_t D :12;
+ //Output power down control bit
+ uint8_t SHDN:1;
+ //Outout gain select bit
+ uint8_t GA :1;
+ //Vref input buffer Control bit
+ uint8_t BUF :1;
+ //DACa or DACb select bit
+ uint8_t AB :1;
+ }bit;
+};
+
+union MCP4922 dac = {0xF7F};
+
+
+//void tick()
+//{
+// static int tcnt, efpls;
+// int epls;
+// float etmppls;
+// tcnt++;
+// if(SW == 0)
+// {
+// wheel.reset();
+// }
+// if(tcnt == 40)
+// {
+// tcnt = 0;
+// epls = wheel.getPulses();
+// etmppls = (float)(epls - efpls);
+// efpls = epls;
+// vcom.printf("\033[%d;%dH", 0, 0);
+// vcom.printf("count :%9d\n", epls);
+// vcom.printf("rev/sec:%3.6f\n", (etmppls / 3600));
+// vcom.printf("DAC.bit:%09d\n", dac.bit.D);
+// }
+//// cs = 0;
+//// spi.write(dac.command);
+//// cs = 1;
+//}
+
+int main()
+{
+ uint8_t i = 0;
+ int epls[2] = {0, 0};
+ float rps = 0;
+
+// flip.attach(tick, (PIDRATE / 2));
+ SW.mode(PullUp);
+ mdrv = CW;
+ cs = 1;
+ dac.bit.AB = 0;
+ dac.bit.BUF = 1;
+ dac.bit.GA = 1;
+ dac.bit.SHDN = 1;
+ dac.bit.D = 0;
+ spi.format(16,0);
+ spi.frequency(20000000);
+ cs = 0;
+ spi.write(dac.command);
+ cs = 1;
+ //Revolution per second input from 0.0 to 50.0rev/sec
+ controller.setInputLimits(0.0, 30.0);
+ //DAC output from 0.0 to 4096.0
+ controller.setOutputLimits(0.0, 4095.0);
+ //If there's a bias.
+ controller.setBias(1000.0);
+ controller.setMode(AUTO_MODE);
+ //We want the process variable to be 12rev/sec
+ controller.setSetPoint(12.0);
+ while(SW == 1)
+ {
+ if(i == 100)
+ {
+ i = 0;
+ vcom.printf("\033[%d;%dH", 0, 0);
+ vcom.printf("DAC.d :%012d\n", dac.bit.D);
+ vcom.printf("rps :%12.4f", rps);
+ }
+ i++;
+ gled = i;
+
+ epls[1] = wheel.getPulses();
+ rps = ((float)(epls[1] - epls[0]) / PIDRATE) / 3600;
+ epls[0] = epls[1];
+ controller.setProcessValue(rps);
+ dac.bit.D = (int)controller.compute();
+ cs = 0;
+ spi.write(dac.command);
+ cs = 1;
+ wait(PIDRATE);
+ }
+ pidsetup();
+ i = 0;
+ rps = 0;
+ epls[0] = 0;
+ timer.reset();
+ timer.start();
+ while(1)
+ {
+ if(SW == 0)
+ {
+ timer.stop();
+ pidsetup();
+ timer.reset();
+ timer.start();
+ }
+ if(i == 10)
+ {
+ vcom.printf("%d,%d\n" , timer.read_ms(), dac.bit.D);
+ i = 0;
+ }
+ i++;
+ gled = i;
+ epls[1] = wheel.getPulses();
+ rps = ((float)(epls[1] - epls[0]) / PIDRATE) / 3600;
+ epls[0] = epls[1];
+ controller.setProcessValue(rps);
+ dac.bit.D = (int)controller.compute();
+ cs = 0;
+ spi.write(dac.command);
+ cs = 1;
+ wait(PIDRATE);
+
+ }
+
+}
+
+void pidsetup()
+{
+ char str[64] = {'\0'}, *erstr;
+ float tmp[3];
+ dac.bit.D = 0;
+ cs = 0;
+ spi.write(dac.command);
+ cs = 1;
+ controller.reset();
+ vcom.printf("\033[2J");
+ vcom.printf("\033[%d;%dH", 0, 0);
+
+ //Output bias
+ vcom.printf("Input the bias for the controller output\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[0] = strtof(str, &erstr);
+ controller.setBias(tmp[0]);
+ vcom.printf("Output bias : %15f\n", tmp[0]);
+
+ //Input limits
+ vcom.printf("Input the minimum inputlimit\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[0] = strtof(str, &erstr);
+ vcom.printf("Minimum input limit : %15f\n", tmp[0]);
+ vcom.printf("Input the maximum inputlimit\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[1] = strtof(str, &erstr);
+ vcom.printf("Maximum input limit : %15f\n", tmp[1]);
+ controller.setInputLimits(tmp[0], tmp[1]);
+
+ //Output limits
+ vcom.printf("Input the minimum outputlimit\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[0] = strtof(str, &erstr);
+ vcom.printf("Minimum output limit : %15f\n", tmp[0]);
+ vcom.printf("Input the maximum outputlimit\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[1] = strtof(str, &erstr);
+ vcom.printf("Maximum output limit : %15f\n", tmp[1]);
+ controller.setOutputLimits(tmp[0], tmp[1]);
+
+ //Setpoint
+ vcom.printf("Input the setpoint\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[0] = strtof(str, &erstr);
+ controller.setSetPoint(tmp[0]);
+ vcom.printf("Setpoint : %15f\n", tmp[0]);
+
+ //tuning parameter
+ vcom.printf("Input the proportional gain\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[0] = strtof(str, &erstr);
+ vcom.printf("proportional gain : %15f\n", tmp[0]);
+ vcom.printf("Input the integral gain\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[1] = strtof(str, &erstr);
+ vcom.printf("integral gain : %15f\n", tmp[1]);
+ vcom.printf("Input the derivative gain\n");
+ vcom.printf("within 15 figures.\n");
+ vcom.scanf("%s", str);
+ tmp[2] = strtof(str, &erstr);
+ vcom.printf("derivative gain : %15f\n", tmp[2]);
+ controller.setTunings(tmp[0], tmp[1], tmp[2]);
+
+ //interval
+// vcom.printf("Input the interval seconds\n");
+// vcom.printf("within 15 figures.\n");
+// vcom.scanf("%s", str);
+// tmp[0] = strtof(str, &erstr);
+ controller.setInterval(PIDRATE);
+ vcom.printf("\033[2J");
+
+
+
+ //PID mode
+ controller.setMode(AUTO_MODE);
+}
\ No newline at end of file