2016 NTHU Robo / Mbed 2 deprecated Nucleo_DCMotor_test

vr.1

Dependencies:   mbed

main.cpp@0:9ed7b8e278fa, 2016-04-18 (annotated)

Committer:
tim65132006
Date:
Mon Apr 18 05:28:06 2016 +0000
Revision:
0:9ed7b8e278fa
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tim65132006 0:9ed7b8e278fa 1 /*LAB_DCMotor*/
tim65132006 0:9ed7b8e278fa 2 #include "mbed.h"
tim65132006 0:9ed7b8e278fa 3
tim65132006 0:9ed7b8e278fa 4 //The number will be compiled as type "double" in default
tim65132006 0:9ed7b8e278fa 5 //Add a "f" after the number can make it compiled as type "float"
tim65132006 0:9ed7b8e278fa 6 #define Ts 0.01f //period of timer1 (s)
tim65132006 0:9ed7b8e278fa 7 #define Kp 0.053f
tim65132006 0:9ed7b8e278fa 8 #define Ki 0.013f
tim65132006 0:9ed7b8e278fa 9
tim65132006 0:9ed7b8e278fa 10 PwmOut pwm1(D7);
tim65132006 0:9ed7b8e278fa 11 PwmOut pwm1n(D11);
tim65132006 0:9ed7b8e278fa 12 PwmOut pwm2(D8);
tim65132006 0:9ed7b8e278fa 13 PwmOut pwm2n(A3);
tim65132006 0:9ed7b8e278fa 14
tim65132006 0:9ed7b8e278fa 15 DigitalOut led1(A4);
tim65132006 0:9ed7b8e278fa 16 DigitalOut led2(A5);
tim65132006 0:9ed7b8e278fa 17
tim65132006 0:9ed7b8e278fa 18 //Motor1 sensor
tim65132006 0:9ed7b8e278fa 19 InterruptIn HallA_1(A1);
tim65132006 0:9ed7b8e278fa 20 InterruptIn HallB_1(A2);
tim65132006 0:9ed7b8e278fa 21 //Motor2 sensor
tim65132006 0:9ed7b8e278fa 22 InterruptIn HallA_2(D13);
tim65132006 0:9ed7b8e278fa 23 InterruptIn HallB_2(D12);
tim65132006 0:9ed7b8e278fa 24
tim65132006 0:9ed7b8e278fa 25 Serial bt(D10,D2);
tim65132006 0:9ed7b8e278fa 26
tim65132006 0:9ed7b8e278fa 27 Ticker timer1;
tim65132006 0:9ed7b8e278fa 28 void timer1_interrupt(void);
tim65132006 0:9ed7b8e278fa 29 void CN_interrupt(void);
tim65132006 0:9ed7b8e278fa 30
tim65132006 0:9ed7b8e278fa 31 void init_TIMER(void);
tim65132006 0:9ed7b8e278fa 32 void init_PWM(void);
tim65132006 0:9ed7b8e278fa 33 void init_CN(void);
tim65132006 0:9ed7b8e278fa 34
tim65132006 0:9ed7b8e278fa 35 void set_rpm(void);//bluetooth in
tim65132006 0:9ed7b8e278fa 36
tim65132006 0:9ed7b8e278fa 37 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
tim65132006 0:9ed7b8e278fa 38 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
tim65132006 0:9ed7b8e278fa 39 int8_t state_1_dec = 0,state_2_dec = 0;//decide foward or inverse
tim65132006 0:9ed7b8e278fa 40
tim65132006 0:9ed7b8e278fa 41 int v1Count = 0;
tim65132006 0:9ed7b8e278fa 42 int v2Count = 0;
tim65132006 0:9ed7b8e278fa 43
tim65132006 0:9ed7b8e278fa 44 double v1 = 0.0, v1_ref = 0.0;
tim65132006 0:9ed7b8e278fa 45 double v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
tim65132006 0:9ed7b8e278fa 46 double v2 = 0.0, v2_ref = 0.0;
tim65132006 0:9ed7b8e278fa 47 double v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
tim65132006 0:9ed7b8e278fa 48
tim65132006 0:9ed7b8e278fa 49 int sec_1 = 0, sec_2 = 0;
tim65132006 0:9ed7b8e278fa 50 //////////command/////////////
tim65132006 0:9ed7b8e278fa 51 char RL;
tim65132006 0:9ed7b8e278fa 52 double char_1;//藍芽讀到的第一位數字元
tim65132006 0:9ed7b8e278fa 53 double char_2;//藍芽讀到的第二位數字元
tim65132006 0:9ed7b8e278fa 54
tim65132006 0:9ed7b8e278fa 55 double a;//個位數
tim65132006 0:9ed7b8e278fa 56 double b;//十位數
tim65132006 0:9ed7b8e278fa 57 //////////////////////////////
tim65132006 0:9ed7b8e278fa 58
tim65132006 0:9ed7b8e278fa 59 int main() {
tim65132006 0:9ed7b8e278fa 60
tim65132006 0:9ed7b8e278fa 61 init_TIMER();
tim65132006 0:9ed7b8e278fa 62 init_PWM();
tim65132006 0:9ed7b8e278fa 63 init_CN();
tim65132006 0:9ed7b8e278fa 64 bt.baud(115200);
tim65132006 0:9ed7b8e278fa 65
tim65132006 0:9ed7b8e278fa 66 v1_ref = 0.0;
tim65132006 0:9ed7b8e278fa 67 v2_ref = 0.0;
tim65132006 0:9ed7b8e278fa 68
tim65132006 0:9ed7b8e278fa 69 while(1)
tim65132006 0:9ed7b8e278fa 70 {
tim65132006 0:9ed7b8e278fa 71
tim65132006 0:9ed7b8e278fa 72 set_rpm();
tim65132006 0:9ed7b8e278fa 73
tim65132006 0:9ed7b8e278fa 74 }
tim65132006 0:9ed7b8e278fa 75 }
tim65132006 0:9ed7b8e278fa 76
tim65132006 0:9ed7b8e278fa 77 void timer1_interrupt(void)
tim65132006 0:9ed7b8e278fa 78 {
tim65132006 0:9ed7b8e278fa 79 //Motor 1
tim65132006 0:9ed7b8e278fa 80 v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
tim65132006 0:9ed7b8e278fa 81 v1Count = 0;
tim65132006 0:9ed7b8e278fa 82
tim65132006 0:9ed7b8e278fa 83 ///code for PI control///
tim65132006 0:9ed7b8e278fa 84
tim65132006 0:9ed7b8e278fa 85 v1_err = v1_ref - v1;
tim65132006 0:9ed7b8e278fa 86 v1_ierr = v1_ierr + v1_err;
tim65132006 0:9ed7b8e278fa 87 PIout_1 = Kp * v1_err + Ki * Ts * v1_ierr;
tim65132006 0:9ed7b8e278fa 88
tim65132006 0:9ed7b8e278fa 89 /////////////////////////
tim65132006 0:9ed7b8e278fa 90
tim65132006 0:9ed7b8e278fa 91 if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
tim65132006 0:9ed7b8e278fa 92 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
tim65132006 0:9ed7b8e278fa 93 pwm1.write(PIout_1 + 0.5f);
tim65132006 0:9ed7b8e278fa 94 TIM1->CCER |= 0x4;
tim65132006 0:9ed7b8e278fa 95
tim65132006 0:9ed7b8e278fa 96
tim65132006 0:9ed7b8e278fa 97 //Motor 2
tim65132006 0:9ed7b8e278fa 98 v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
tim65132006 0:9ed7b8e278fa 99 v2Count = 0;
tim65132006 0:9ed7b8e278fa 100
tim65132006 0:9ed7b8e278fa 101 ///code for PI control///
tim65132006 0:9ed7b8e278fa 102 v2_err = v2_ref - v2;
tim65132006 0:9ed7b8e278fa 103 v2_ierr = v2_ierr + v2_err;
tim65132006 0:9ed7b8e278fa 104 PIout_2 = Kp * v2_err + Ki * Ts * v2_ierr;
tim65132006 0:9ed7b8e278fa 105
tim65132006 0:9ed7b8e278fa 106 /////////////////////////
tim65132006 0:9ed7b8e278fa 107
tim65132006 0:9ed7b8e278fa 108 if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
tim65132006 0:9ed7b8e278fa 109 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
tim65132006 0:9ed7b8e278fa 110 pwm2.write(PIout_2 + 0.5f);
tim65132006 0:9ed7b8e278fa 111 TIM1->CCER |= 0x40;
tim65132006 0:9ed7b8e278fa 112 }
tim65132006 0:9ed7b8e278fa 113
tim65132006 0:9ed7b8e278fa 114 void CN_interrupt(void)
tim65132006 0:9ed7b8e278fa 115 {
tim65132006 0:9ed7b8e278fa 116 //Motor 1
tim65132006 0:9ed7b8e278fa 117 stateA_1 = HallA_1.read();
tim65132006 0:9ed7b8e278fa 118 stateB_1 = HallB_1.read();
tim65132006 0:9ed7b8e278fa 119
tim65132006 0:9ed7b8e278fa 120 state_1 = stateA_1 * 2 + stateB_1;
tim65132006 0:9ed7b8e278fa 121
tim65132006 0:9ed7b8e278fa 122 state_1_dec = state_1 - state_1_old; //判別正反
tim65132006 0:9ed7b8e278fa 123
tim65132006 0:9ed7b8e278fa 124
tim65132006 0:9ed7b8e278fa 125 ///code for state determination///
tim65132006 0:9ed7b8e278fa 126 if(state_1_dec == 1 || state_1_dec == -3 ) //Forward
tim65132006 0:9ed7b8e278fa 127 {
tim65132006 0:9ed7b8e278fa 128 v1Count++;
tim65132006 0:9ed7b8e278fa 129 }
tim65132006 0:9ed7b8e278fa 130
tim65132006 0:9ed7b8e278fa 131 else if(state_1_dec == -1 || state_1_dec == 3 ) //Inverse
tim65132006 0:9ed7b8e278fa 132 {
tim65132006 0:9ed7b8e278fa 133 v1Count--;
tim65132006 0:9ed7b8e278fa 134 }
tim65132006 0:9ed7b8e278fa 135
tim65132006 0:9ed7b8e278fa 136 state_1_old = state_1;
tim65132006 0:9ed7b8e278fa 137
tim65132006 0:9ed7b8e278fa 138
tim65132006 0:9ed7b8e278fa 139 //Motor 2
tim65132006 0:9ed7b8e278fa 140 stateA_2 = HallA_2.read();
tim65132006 0:9ed7b8e278fa 141 stateB_2 = HallB_2.read();
tim65132006 0:9ed7b8e278fa 142
tim65132006 0:9ed7b8e278fa 143 state_2 = stateA_2 * 2 + stateB_2;
tim65132006 0:9ed7b8e278fa 144
tim65132006 0:9ed7b8e278fa 145 state_2_dec = state_2 - state_2_old; //判別正反
tim65132006 0:9ed7b8e278fa 146
tim65132006 0:9ed7b8e278fa 147
tim65132006 0:9ed7b8e278fa 148 ///code for state determination///
tim65132006 0:9ed7b8e278fa 149 if(state_2_dec == 1 || state_2_dec == -3 ) //Forward
tim65132006 0:9ed7b8e278fa 150 {
tim65132006 0:9ed7b8e278fa 151 v1Count++;
tim65132006 0:9ed7b8e278fa 152 }
tim65132006 0:9ed7b8e278fa 153
tim65132006 0:9ed7b8e278fa 154 else if(state_2_dec == -1 || state_2_dec == 3 ) //Inverse
tim65132006 0:9ed7b8e278fa 155 {
tim65132006 0:9ed7b8e278fa 156 v1Count--;
tim65132006 0:9ed7b8e278fa 157 }
tim65132006 0:9ed7b8e278fa 158
tim65132006 0:9ed7b8e278fa 159 state_2_old = state_2;
tim65132006 0:9ed7b8e278fa 160 }
tim65132006 0:9ed7b8e278fa 161
tim65132006 0:9ed7b8e278fa 162 void init_TIMER(void)
tim65132006 0:9ed7b8e278fa 163 {
tim65132006 0:9ed7b8e278fa 164 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
tim65132006 0:9ed7b8e278fa 165 }
tim65132006 0:9ed7b8e278fa 166
tim65132006 0:9ed7b8e278fa 167 void init_PWM(void)
tim65132006 0:9ed7b8e278fa 168 {
tim65132006 0:9ed7b8e278fa 169 pwm1.period_us(50);
tim65132006 0:9ed7b8e278fa 170 pwm1.write(0.5);
tim65132006 0:9ed7b8e278fa 171 TIM1->CCER |= 0x4;
tim65132006 0:9ed7b8e278fa 172
tim65132006 0:9ed7b8e278fa 173 pwm2.period_us(50);
tim65132006 0:9ed7b8e278fa 174 pwm2.write(0.5);
tim65132006 0:9ed7b8e278fa 175 TIM1->CCER |= 0x40;
tim65132006 0:9ed7b8e278fa 176 }
tim65132006 0:9ed7b8e278fa 177
tim65132006 0:9ed7b8e278fa 178 void init_CN(void)
tim65132006 0:9ed7b8e278fa 179 {
tim65132006 0:9ed7b8e278fa 180 HallA_1.rise(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 181 HallA_1.fall(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 182 HallB_1.rise(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 183 HallB_1.fall(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 184
tim65132006 0:9ed7b8e278fa 185 HallA_2.rise(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 186 HallA_2.fall(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 187 HallB_2.rise(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 188 HallB_2.fall(&CN_interrupt);
tim65132006 0:9ed7b8e278fa 189
tim65132006 0:9ed7b8e278fa 190 stateA_1 = HallA_1.read();
tim65132006 0:9ed7b8e278fa 191 stateB_1 = HallB_1.read();
tim65132006 0:9ed7b8e278fa 192 stateA_2 = HallA_2.read();
tim65132006 0:9ed7b8e278fa 193 stateB_2 = HallB_2.read();
tim65132006 0:9ed7b8e278fa 194 }
tim65132006 0:9ed7b8e278fa 195
tim65132006 0:9ed7b8e278fa 196 void set_rpm(void)
tim65132006 0:9ed7b8e278fa 197 {
tim65132006 0:9ed7b8e278fa 198
tim65132006 0:9ed7b8e278fa 199 RL = bt.getc();
tim65132006 0:9ed7b8e278fa 200
tim65132006 0:9ed7b8e278fa 201 switch( RL )
tim65132006 0:9ed7b8e278fa 202 {
tim65132006 0:9ed7b8e278fa 203 case 'r':
tim65132006 0:9ed7b8e278fa 204 bt.printf("%c",RL);
tim65132006 0:9ed7b8e278fa 205
tim65132006 0:9ed7b8e278fa 206 char_1 = bt.getc();
tim65132006 0:9ed7b8e278fa 207 a = char_1 - 48;
tim65132006 0:9ed7b8e278fa 208
tim65132006 0:9ed7b8e278fa 209 if(a > -1 && a < 10)
tim65132006 0:9ed7b8e278fa 210 {
tim65132006 0:9ed7b8e278fa 211 char_2 = bt.getc();
tim65132006 0:9ed7b8e278fa 212 b = char_2 - 48;
tim65132006 0:9ed7b8e278fa 213 v1_ierr = 0;
tim65132006 0:9ed7b8e278fa 214 v1_ref = 10*a+b;
tim65132006 0:9ed7b8e278fa 215
tim65132006 0:9ed7b8e278fa 216 bt.printf("%f\n",v1_ref);
tim65132006 0:9ed7b8e278fa 217 }
tim65132006 0:9ed7b8e278fa 218
tim65132006 0:9ed7b8e278fa 219 else
tim65132006 0:9ed7b8e278fa 220 {
tim65132006 0:9ed7b8e278fa 221 bt.printf("please key in number");
tim65132006 0:9ed7b8e278fa 222 }
tim65132006 0:9ed7b8e278fa 223
tim65132006 0:9ed7b8e278fa 224
tim65132006 0:9ed7b8e278fa 225 while(v1 < v1_ref*0.95 || v1 > v1_ref*1.05 )
tim65132006 0:9ed7b8e278fa 226 {
tim65132006 0:9ed7b8e278fa 227 bt.printf("state %d ",state_1_dec);
tim65132006 0:9ed7b8e278fa 228 bt.printf("%f ",v1);
tim65132006 0:9ed7b8e278fa 229 sec_1++;
tim65132006 0:9ed7b8e278fa 230 bt.printf("%d\n",sec_1);
tim65132006 0:9ed7b8e278fa 231 wait(1);
tim65132006 0:9ed7b8e278fa 232 }
tim65132006 0:9ed7b8e278fa 233 sec_1 = 0;
tim65132006 0:9ed7b8e278fa 234 bt.printf("end %d\n",sec_1);
tim65132006 0:9ed7b8e278fa 235
tim65132006 0:9ed7b8e278fa 236 break;
tim65132006 0:9ed7b8e278fa 237 /////////////////////////////////
tim65132006 0:9ed7b8e278fa 238
tim65132006 0:9ed7b8e278fa 239 case 'l':
tim65132006 0:9ed7b8e278fa 240 bt.printf("%c",RL);
tim65132006 0:9ed7b8e278fa 241
tim65132006 0:9ed7b8e278fa 242 char_1 = bt.getc();
tim65132006 0:9ed7b8e278fa 243 a = char_1 - 48;
tim65132006 0:9ed7b8e278fa 244
tim65132006 0:9ed7b8e278fa 245 if(a > -1 && a < 10)
tim65132006 0:9ed7b8e278fa 246 {
tim65132006 0:9ed7b8e278fa 247 char_2 = bt.getc();
tim65132006 0:9ed7b8e278fa 248 b = char_2 - 48;
tim65132006 0:9ed7b8e278fa 249 v2_ierr = 0;
tim65132006 0:9ed7b8e278fa 250 v2_ref = 10*a+b;
tim65132006 0:9ed7b8e278fa 251
tim65132006 0:9ed7b8e278fa 252 bt.printf("%f\n",v2_ref);
tim65132006 0:9ed7b8e278fa 253 }
tim65132006 0:9ed7b8e278fa 254
tim65132006 0:9ed7b8e278fa 255 else
tim65132006 0:9ed7b8e278fa 256 {
tim65132006 0:9ed7b8e278fa 257 bt.printf("please key in number");
tim65132006 0:9ed7b8e278fa 258 }
tim65132006 0:9ed7b8e278fa 259
tim65132006 0:9ed7b8e278fa 260
tim65132006 0:9ed7b8e278fa 261 while(v2 < v2_ref*0.95 || v2 > v2_ref*1.05 )
tim65132006 0:9ed7b8e278fa 262 {
tim65132006 0:9ed7b8e278fa 263 bt.printf("%f ",v2);
tim65132006 0:9ed7b8e278fa 264 sec_2++;
tim65132006 0:9ed7b8e278fa 265 bt.printf("%d\n",sec_2);
tim65132006 0:9ed7b8e278fa 266 wait(1);
tim65132006 0:9ed7b8e278fa 267 }
tim65132006 0:9ed7b8e278fa 268 sec_2 = 0;
tim65132006 0:9ed7b8e278fa 269 bt.printf("end %d\n",sec_2);
tim65132006 0:9ed7b8e278fa 270
tim65132006 0:9ed7b8e278fa 271 break;
tim65132006 0:9ed7b8e278fa 272 }
tim65132006 0:9ed7b8e278fa 273
tim65132006 0:9ed7b8e278fa 274 }
tim65132006 0:9ed7b8e278fa 275