Kojiro Iizuka / PID_Control_SU

a

Dependencies:   PID QEI USBDevice mbed

Fork of PID_Control_SU by naoto tanaka

main.cpp@0:b2973a157cb6, 2014-04-15 (annotated)

Committer:
NT32
Date:
Tue Apr 15 02:02:38 2014 +0000
Revision:
0:b2973a157cb6
Child:
1:6d5c35b995fb
motor control for practice

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