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

main.cpp@0:2ac10e7b6b03, 2016-05-24 (annotated)

Committer:
roger5641
Date:
Tue May 24 05:29:47 2016 +0000
Revision:
0:2ac10e7b6b03
Child:
8:079c3326816e
robot

Who changed what in which revision?

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