Robotics Term Project / Mbed 2 deprecated Robotics_Lab_DCMotor

DC_Motor

Dependencies:   mbed

Fork of Robotics_Lab_DCMotor by LDSC_Robotics

main.cpp@2:9caf46a5fe5a, 2016-05-25 (annotated)

Committer:
roger5641
Date:
Wed May 25 07:02:22 2016 +0000
Revision:
2:9caf46a5fe5a
Parent:
1:6747911cdd90 
DC_Motor

Who changed what in which revision?

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