Robotics LAB DCMotor
Dependencies: mbed
Fork of Robotics_Lab_DCMotor by
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
- Child:
- 3:0856a9fa97ec
DC_Motor
Who changed what in which revision?
User | Revision | Line number | New 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 | } |