naoto tanaka / PID_Control_SU

practice for PID control

Dependencies:   PID QEI USBDevice mbed

main.cpp@5:f668eb3ee52a, 2014-05-02 (annotated)

Committer:
NT32
Date:
Fri May 02 04:47:47 2014 +0000
Revision:
5:f668eb3ee52a
Parent:
4:adf167473520
Child:
6:1250bc2df075
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NT32 0:b2973a157cb6 1 #include "mbed.h"
NT32 0:b2973a157cb6 2 #include "USBSerial.h"
NT32 0:b2973a157cb6 3 #include "QEI.h"
NT32 0:b2973a157cb6 4 #include "PID.h"
NT32 4:adf167473520 5 #include "SDFileSystem.h"
NT32 0:b2973a157cb6 6
NT32 0:b2973a157cb6 7 #define PIDRATE 0.01
NT32 0:b2973a157cb6 8 #define CW 0x01
NT32 0:b2973a157cb6 9 #define CCW 0x02
NT32 0:b2973a157cb6 10 #define STOP 0x03
NT32 0:b2973a157cb6 11 #define FREE 0x00
NT32 0:b2973a157cb6 12
NT32 0:b2973a157cb6 13 #define kc 2.8
NT32 0:b2973a157cb6 14 #define ti 16.0
NT32 0:b2973a157cb6 15 #define td 0.0
NT32 0:b2973a157cb6 16
NT32 0:b2973a157cb6 17 USBSerial vcom;
NT32 0:b2973a157cb6 18 SPI spi(P0_21, NC, P1_20);
NT32 0:b2973a157cb6 19 DigitalOut cs(P1_23);
NT32 0:b2973a157cb6 20
NT32 4:adf167473520 21
NT32 0:b2973a157cb6 22 Timer timer;
NT32 0:b2973a157cb6 23
NT32 0:b2973a157cb6 24 PID controller(kc, ti, td, PIDRATE);
NT32 0:b2973a157cb6 25
NT32 0:b2973a157cb6 26 QEI wheel (P1_15, P0_19, NC, 1, QEI::X4_ENCODING);
NT32 0:b2973a157cb6 27
NT32 0:b2973a157cb6 28 BusOut gled(P0_8, P0_9);
NT32 0:b2973a157cb6 29 BusOut mdrv(P1_27, P1_26);
NT32 0:b2973a157cb6 30
NT32 0:b2973a157cb6 31 DigitalIn SW(P0_1);
NT32 0:b2973a157cb6 32
NT32 2:b46f1a4c42fb 33 void initialize();
NT32 0:b2973a157cb6 34 void pidsetup();
NT32 0:b2973a157cb6 35
NT32 0:b2973a157cb6 36 union MCP4922
NT32 0:b2973a157cb6 37 {
NT32 0:b2973a157cb6 38 uint16_t command;
NT32 0:b2973a157cb6 39 struct
NT32 0:b2973a157cb6 40 {
NT32 0:b2973a157cb6 41 //DAC data bits
NT32 0:b2973a157cb6 42 uint16_t D :12;
NT32 0:b2973a157cb6 43 //Output power down control bit
NT32 0:b2973a157cb6 44 uint8_t SHDN:1;
NT32 0:b2973a157cb6 45 //Outout gain select bit
NT32 0:b2973a157cb6 46 uint8_t GA :1;
NT32 0:b2973a157cb6 47 //Vref input buffer Control bit
NT32 0:b2973a157cb6 48 uint8_t BUF :1;
NT32 0:b2973a157cb6 49 //DACa or DACb select bit
NT32 0:b2973a157cb6 50 uint8_t AB :1;
NT32 0:b2973a157cb6 51 }bit;
NT32 0:b2973a157cb6 52 };
NT32 0:b2973a157cb6 53
NT32 0:b2973a157cb6 54 union MCP4922 dac = {0xF7F};
NT32 0:b2973a157cb6 55
NT32 0:b2973a157cb6 56 int main()
NT32 0:b2973a157cb6 57 {
NT32 0:b2973a157cb6 58 uint8_t i = 0;
NT32 0:b2973a157cb6 59 int epls[2] = {0, 0};
NT32 0:b2973a157cb6 60 float rps = 0;
NT32 3:e9aeee8b41e4 61
NT32 3:e9aeee8b41e4 62 //IO,DAconverter and PID parameters configuretion.
NT32 2:b46f1a4c42fb 63 initialize();
NT32 3:e9aeee8b41e4 64
NT32 3:e9aeee8b41e4 65 //this block is the demonstration of PID control until BOOT switch is pushed.
NT32 0:b2973a157cb6 66 while(SW == 1)
NT32 0:b2973a157cb6 67 {
NT32 3:e9aeee8b41e4 68 //this block is the outputs DAC data and revolution per second of motor.
NT32 0:b2973a157cb6 69 if(i == 100)
NT32 0:b2973a157cb6 70 {
NT32 0:b2973a157cb6 71 i = 0;
NT32 0:b2973a157cb6 72 vcom.printf("\033[%d;%dH", 0, 0);
NT32 0:b2973a157cb6 73 vcom.printf("DAC.d :%012d\n", dac.bit.D);
NT32 0:b2973a157cb6 74 vcom.printf("rps :%12.4f", rps);
NT32 0:b2973a157cb6 75 }
NT32 3:e9aeee8b41e4 76
NT32 3:e9aeee8b41e4 77 //led is blinked to display the loop of PID control.
NT32 0:b2973a157cb6 78 i++;
NT32 0:b2973a157cb6 79 gled = i;
NT32 0:b2973a157cb6 80
NT32 3:e9aeee8b41e4 81 //calculate rps.
NT32 0:b2973a157cb6 82 epls[1] = wheel.getPulses();
NT32 0:b2973a157cb6 83 rps = ((float)(epls[1] - epls[0]) / PIDRATE) / 3600;
NT32 0:b2973a157cb6 84 epls[0] = epls[1];
NT32 0:b2973a157cb6 85 controller.setProcessValue(rps);
NT32 0:b2973a157cb6 86 dac.bit.D = (int)controller.compute();
NT32 3:e9aeee8b41e4 87
NT32 3:e9aeee8b41e4 88 //send a command to change output of voltage.
NT32 0:b2973a157cb6 89 cs = 0;
NT32 0:b2973a157cb6 90 spi.write(dac.command);
NT32 0:b2973a157cb6 91 cs = 1;
NT32 0:b2973a157cb6 92 wait(PIDRATE);
NT32 0:b2973a157cb6 93 }
NT32 0:b2973a157cb6 94 pidsetup();
NT32 0:b2973a157cb6 95 i = 0;
NT32 0:b2973a157cb6 96 rps = 0;
NT32 0:b2973a157cb6 97 epls[0] = 0;
NT32 0:b2973a157cb6 98 timer.reset();
NT32 0:b2973a157cb6 99 timer.start();
NT32 3:e9aeee8b41e4 100 vcom.printf("%d,%d,%f\n" , timer.read_ms(), dac.bit.D, rps);
NT32 3:e9aeee8b41e4 101 i = 0;
NT32 0:b2973a157cb6 102 while(1)
NT32 0:b2973a157cb6 103 {
NT32 0:b2973a157cb6 104 if(SW == 0)
NT32 0:b2973a157cb6 105 {
NT32 0:b2973a157cb6 106 timer.stop();
NT32 0:b2973a157cb6 107 pidsetup();
NT32 0:b2973a157cb6 108 timer.reset();
NT32 0:b2973a157cb6 109 timer.start();
NT32 4:adf167473520 110 vcom.printf("%d,%d,%f\n" , timer.read_ms(), dac.bit.D, rps);
NT32 0:b2973a157cb6 111 }
NT32 2:b46f1a4c42fb 112
NT32 0:b2973a157cb6 113 i++;
NT32 0:b2973a157cb6 114 gled = i;
NT32 0:b2973a157cb6 115 epls[1] = wheel.getPulses();
NT32 0:b2973a157cb6 116 rps = ((float)(epls[1] - epls[0]) / PIDRATE) / 3600;
NT32 0:b2973a157cb6 117 epls[0] = epls[1];
NT32 0:b2973a157cb6 118 controller.setProcessValue(rps);
NT32 0:b2973a157cb6 119 dac.bit.D = (int)controller.compute();
NT32 0:b2973a157cb6 120 cs = 0;
NT32 0:b2973a157cb6 121 spi.write(dac.command);
NT32 0:b2973a157cb6 122 cs = 1;
NT32 2:b46f1a4c42fb 123 if(i == 10)
NT32 2:b46f1a4c42fb 124 {
NT32 2:b46f1a4c42fb 125 vcom.printf("%d,%d,%f\n" , timer.read_ms(), dac.bit.D, rps);
NT32 2:b46f1a4c42fb 126 i = 0;
NT32 2:b46f1a4c42fb 127 }
NT32 0:b2973a157cb6 128 wait(PIDRATE);
NT32 0:b2973a157cb6 129
NT32 0:b2973a157cb6 130 }
NT32 0:b2973a157cb6 131
NT32 0:b2973a157cb6 132 }
NT32 0:b2973a157cb6 133
NT32 2:b46f1a4c42fb 134 void initialize()
NT32 2:b46f1a4c42fb 135 {
NT32 2:b46f1a4c42fb 136 SW.mode(PullUp);
NT32 2:b46f1a4c42fb 137 mdrv = CW;
NT32 2:b46f1a4c42fb 138 cs = 1;
NT32 2:b46f1a4c42fb 139 dac.bit.AB = 0;
NT32 2:b46f1a4c42fb 140 dac.bit.BUF = 1;
NT32 2:b46f1a4c42fb 141 dac.bit.GA = 1;
NT32 2:b46f1a4c42fb 142 dac.bit.SHDN = 1;
NT32 2:b46f1a4c42fb 143 dac.bit.D = 0;
NT32 2:b46f1a4c42fb 144 spi.format(16,0);
NT32 2:b46f1a4c42fb 145 spi.frequency(20000000);
NT32 2:b46f1a4c42fb 146 cs = 0;
NT32 2:b46f1a4c42fb 147 spi.write(dac.command);
NT32 2:b46f1a4c42fb 148 cs = 1;
NT32 2:b46f1a4c42fb 149 //Revolution per second input from 0.0 to 50.0rev/sec
NT32 2:b46f1a4c42fb 150 controller.setInputLimits(0.0, 30.0);
NT32 2:b46f1a4c42fb 151 //DAC output from 0.0 to 4096.0
NT32 2:b46f1a4c42fb 152 controller.setOutputLimits(0.0, 4095.0);
NT32 2:b46f1a4c42fb 153 //If there's a bias.
NT32 2:b46f1a4c42fb 154 controller.setBias(1000.0);
NT32 2:b46f1a4c42fb 155 controller.setMode(AUTO_MODE);
NT32 2:b46f1a4c42fb 156 //We want the process variable to be 12rev/sec
NT32 2:b46f1a4c42fb 157 controller.setSetPoint(12.0);
NT32 2:b46f1a4c42fb 158 }
NT32 2:b46f1a4c42fb 159
NT32 0:b2973a157cb6 160 void pidsetup()
NT32 0:b2973a157cb6 161 {
NT32 0:b2973a157cb6 162 char str[64] = {'\0'}, *erstr;
NT32 0:b2973a157cb6 163 float tmp[3];
NT32 0:b2973a157cb6 164 dac.bit.D = 0;
NT32 0:b2973a157cb6 165 cs = 0;
NT32 0:b2973a157cb6 166 spi.write(dac.command);
NT32 0:b2973a157cb6 167 cs = 1;
NT32 0:b2973a157cb6 168 controller.reset();
NT32 0:b2973a157cb6 169 vcom.printf("\033[2J");
NT32 0:b2973a157cb6 170 vcom.printf("\033[%d;%dH", 0, 0);
NT32 0:b2973a157cb6 171
NT32 0:b2973a157cb6 172 //Output bias
NT32 1:6d5c35b995fb 173 // vcom.printf("Input the bias for the controller output\n");
NT32 1:6d5c35b995fb 174 // vcom.printf("within 15 figures.\n");
NT32 1:6d5c35b995fb 175 // vcom.scanf("%s", str);
NT32 1:6d5c35b995fb 176 // tmp[0] = strtof(str, &erstr);
NT32 1:6d5c35b995fb 177 // controller.setBias(tmp[0]);
NT32 1:6d5c35b995fb 178 // vcom.printf("Output bias : %15f\n", tmp[0]);
NT32 1:6d5c35b995fb 179 controller.setBias(200);
NT32 0:b2973a157cb6 180
NT32 0:b2973a157cb6 181 //Input limits
NT32 1:6d5c35b995fb 182 // vcom.printf("Input the minimum inputlimit\n");
NT32 1:6d5c35b995fb 183 // vcom.printf("within 15 figures.\n");
NT32 1:6d5c35b995fb 184 // vcom.scanf("%s", str);
NT32 1:6d5c35b995fb 185 // tmp[0] = strtof(str, &erstr);
NT32 1:6d5c35b995fb 186 // vcom.printf("Minimum input limit : %15f\n", tmp[0]);
NT32 0:b2973a157cb6 187 vcom.printf("Input the maximum inputlimit\n");
NT32 0:b2973a157cb6 188 vcom.printf("within 15 figures.\n");
NT32 0:b2973a157cb6 189 vcom.scanf("%s", str);
NT32 0:b2973a157cb6 190 tmp[1] = strtof(str, &erstr);
NT32 0:b2973a157cb6 191 vcom.printf("Maximum input limit : %15f\n", tmp[1]);
NT32 1:6d5c35b995fb 192 // controller.setInputLimits(tmp[0], tmp[1]);
NT32 1:6d5c35b995fb 193 controller.setInputLimits(0,tmp[1]);
NT32 0:b2973a157cb6 194
NT32 0:b2973a157cb6 195 //Output limits
NT32 1:6d5c35b995fb 196 // vcom.printf("Input the minimum outputlimit\n");
NT32 1:6d5c35b995fb 197 // vcom.printf("within 15 figures.\n");
NT32 1:6d5c35b995fb 198 // vcom.scanf("%s", str);
NT32 1:6d5c35b995fb 199 // tmp[0] = strtof(str, &erstr);
NT32 1:6d5c35b995fb 200 // vcom.printf("Minimum output limit : %15f\n", tmp[0]);
NT32 1:6d5c35b995fb 201 // vcom.printf("Input the maximum outputlimit\n");
NT32 1:6d5c35b995fb 202 // vcom.printf("within 15 figures.\n");
NT32 1:6d5c35b995fb 203 // vcom.scanf("%s", str);
NT32 1:6d5c35b995fb 204 // tmp[1] = strtof(str, &erstr);
NT32 1:6d5c35b995fb 205 // vcom.printf("Maximum output limit : %15f\n", tmp[1]);
NT32 1:6d5c35b995fb 206 // controller.setOutputLimits(tmp[0], tmp[1]);
NT32 1:6d5c35b995fb 207 controller.setOutputLimits(0, 4095);
NT32 0:b2973a157cb6 208
NT32 0:b2973a157cb6 209 //Setpoint
NT32 0:b2973a157cb6 210 vcom.printf("Input the setpoint\n");
NT32 0:b2973a157cb6 211 vcom.printf("within 15 figures.\n");
NT32 0:b2973a157cb6 212 vcom.scanf("%s", str);
NT32 0:b2973a157cb6 213 tmp[0] = strtof(str, &erstr);
NT32 0:b2973a157cb6 214 controller.setSetPoint(tmp[0]);
NT32 0:b2973a157cb6 215 vcom.printf("Setpoint : %15f\n", tmp[0]);
NT32 0:b2973a157cb6 216
NT32 0:b2973a157cb6 217 //tuning parameter
NT32 0:b2973a157cb6 218 vcom.printf("Input the proportional gain\n");
NT32 0:b2973a157cb6 219 vcom.printf("within 15 figures.\n");
NT32 0:b2973a157cb6 220 vcom.scanf("%s", str);
NT32 0:b2973a157cb6 221 tmp[0] = strtof(str, &erstr);
NT32 0:b2973a157cb6 222 vcom.printf("proportional gain : %15f\n", tmp[0]);
NT32 0:b2973a157cb6 223 vcom.printf("Input the integral gain\n");
NT32 0:b2973a157cb6 224 vcom.printf("within 15 figures.\n");
NT32 0:b2973a157cb6 225 vcom.scanf("%s", str);
NT32 0:b2973a157cb6 226 tmp[1] = strtof(str, &erstr);
NT32 0:b2973a157cb6 227 vcom.printf("integral gain : %15f\n", tmp[1]);
NT32 0:b2973a157cb6 228 vcom.printf("Input the derivative gain\n");
NT32 0:b2973a157cb6 229 vcom.printf("within 15 figures.\n");
NT32 0:b2973a157cb6 230 vcom.scanf("%s", str);
NT32 0:b2973a157cb6 231 tmp[2] = strtof(str, &erstr);
NT32 0:b2973a157cb6 232 vcom.printf("derivative gain : %15f\n", tmp[2]);
NT32 0:b2973a157cb6 233 controller.setTunings(tmp[0], tmp[1], tmp[2]);
NT32 0:b2973a157cb6 234
NT32 0:b2973a157cb6 235 //interval
NT32 0:b2973a157cb6 236 // vcom.printf("Input the interval seconds\n");
NT32 0:b2973a157cb6 237 // vcom.printf("within 15 figures.\n");
NT32 0:b2973a157cb6 238 // vcom.scanf("%s", str);
NT32 0:b2973a157cb6 239 // tmp[0] = strtof(str, &erstr);
NT32 0:b2973a157cb6 240 controller.setInterval(PIDRATE);
NT32 0:b2973a157cb6 241 vcom.printf("\033[2J");
NT32 4:adf167473520 242 vcom.printf("\033[%d;%dH", 0, 0);
NT32 5:f668eb3ee52a 243 vcom.printf("Make a logfile that name of \" hoge.csv \"\n");
NT32 4:adf167473520 244 vcom.printf("Push the boot switch to start PID control.");
NT32 4:adf167473520 245
NT32 4:adf167473520 246 while(SW == 1){};
NT32 4:adf167473520 247 wait(0.1);
NT32 4:adf167473520 248 while(SW == 0){};
NT32 4:adf167473520 249
NT32 4:adf167473520 250 vcom.printf("\033[2J");
NT32 4:adf167473520 251 vcom.printf("\033[%d;%dH", 0, 0);
NT32 4:adf167473520 252
NT32 0:b2973a157cb6 253
NT32 0:b2973a157cb6 254
NT32 0:b2973a157cb6 255
NT32 0:b2973a157cb6 256 //PID mode
NT32 0:b2973a157cb6 257 controller.setMode(AUTO_MODE);
NT32 0:b2973a157cb6 258 }