Robotics Term Project / Mbed 2 deprecated Robottics_Motion

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

AI_Friday.cpp@9:b073f4ecb5f9, 2016-05-25 (annotated)

Committer:
smilestone520
Date:
Wed May 25 10:28:20 2016 +0000
Revision:
9:b073f4ecb5f9
Child:
11:44989c0bcea5
original hw

Who changed what in which revision?

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