PME_Police / Mbed 2 deprecated igloo3

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

Fork of Robotics_LAB_motor by NTHUPME_Robotics_2017

main.cpp@1:b222972c5930, 2017-03-21 (annotated)

Committer:
money24617111
Date:
Tue Mar 21 12:18:09 2017 +0000
Revision:
1:b222972c5930
Parent:
0:74ea99c4db88 
Hall sensor algo     Turkey

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hoting 0:74ea99c4db88 1 #include "mbed.h"
hoting 0:74ea99c4db88 2
money24617111 1:b222972c5930 3
money24617111 1:b222972c5930 4 Serial pc(USBTX,USBRX);
money24617111 1:b222972c5930 5
hoting 0:74ea99c4db88 6 //The number will be compiled as type "double" in default
hoting 0:74ea99c4db88 7 //Add a "f" after the number can make it compiled as type "float"
hoting 0:74ea99c4db88 8 #define Ts 0.01f //period of timer1 (s)
hoting 0:74ea99c4db88 9
hoting 0:74ea99c4db88 10 Ticker timer1;
hoting 0:74ea99c4db88 11 // servo motor
hoting 0:74ea99c4db88 12 PwmOut servo_cmd(A0);
hoting 0:74ea99c4db88 13 // DC motor
hoting 0:74ea99c4db88 14 PwmOut pwm1(D7);
hoting 0:74ea99c4db88 15 PwmOut pwm1n(D11);
hoting 0:74ea99c4db88 16 PwmOut pwm2(D8);
hoting 0:74ea99c4db88 17 PwmOut pwm2n(A3);
hoting 0:74ea99c4db88 18
hoting 0:74ea99c4db88 19 // Motor1 sensor
hoting 0:74ea99c4db88 20 InterruptIn HallA(A1);
hoting 0:74ea99c4db88 21 InterruptIn HallB(A2);
hoting 0:74ea99c4db88 22 // Motor2 sensor
hoting 0:74ea99c4db88 23 InterruptIn HallA_2(D13);
hoting 0:74ea99c4db88 24 InterruptIn HallB_2(D12);
hoting 0:74ea99c4db88 25
hoting 0:74ea99c4db88 26 // 函式宣告
hoting 0:74ea99c4db88 27 void init_IO();
hoting 0:74ea99c4db88 28 void init_TIMER();
hoting 0:74ea99c4db88 29 void timer1_ITR();
hoting 0:74ea99c4db88 30 void init_CN();
hoting 0:74ea99c4db88 31 void CN_ITR();
hoting 0:74ea99c4db88 32 void init_PWM();
hoting 0:74ea99c4db88 33
hoting 0:74ea99c4db88 34 // servo motor
hoting 0:74ea99c4db88 35 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
hoting 0:74ea99c4db88 36 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
hoting 0:74ea99c4db88 37 int angle = 0;
hoting 0:74ea99c4db88 38
hoting 0:74ea99c4db88 39 // Hall sensor
hoting 0:74ea99c4db88 40 int HallA_1_state = 0;
hoting 0:74ea99c4db88 41 int HallB_1_state = 0;
hoting 0:74ea99c4db88 42 int state_1 = 0;
hoting 0:74ea99c4db88 43 int state_1_old = 0;
hoting 0:74ea99c4db88 44 int HallA_2_state = 0;
hoting 0:74ea99c4db88 45 int HallB_2_state = 0;
hoting 0:74ea99c4db88 46 int state_2 = 0;
hoting 0:74ea99c4db88 47 int state_2_old = 0;
hoting 0:74ea99c4db88 48
hoting 0:74ea99c4db88 49 // DC motor rotation speed control
hoting 0:74ea99c4db88 50 int speed_count_1 = 0;
hoting 0:74ea99c4db88 51 float rotation_speed_1 = 0.0;
money24617111 1:b222972c5930 52 float rotation_speed_ref_1 = 150;
hoting 0:74ea99c4db88 53 float pwm1_duty = 0.5;
hoting 0:74ea99c4db88 54 float PI_out_1 = 0.0;
hoting 0:74ea99c4db88 55 float err_1 = 0.0;
hoting 0:74ea99c4db88 56 float ierr_1 = 0.0;
hoting 0:74ea99c4db88 57 int speed_count_2 = 0;
hoting 0:74ea99c4db88 58 float rotation_speed_2 = 0.0;
money24617111 1:b222972c5930 59 float rotation_speed_ref_2 = 150;
hoting 0:74ea99c4db88 60 float pwm2_duty = 0.5;
hoting 0:74ea99c4db88 61 float PI_out_2 = 0.0;
hoting 0:74ea99c4db88 62 float err_2 = 0.0;
hoting 0:74ea99c4db88 63 float ierr_2 = 0.0;
hoting 0:74ea99c4db88 64
money24617111 1:b222972c5930 65 float Ki = 0.02;
money24617111 1:b222972c5930 66 float Kp = 0.7;
money24617111 1:b222972c5930 67
hoting 0:74ea99c4db88 68 int main()
hoting 0:74ea99c4db88 69 {
money24617111 1:b222972c5930 70 // FILE *fp = fopen("C:/Users/user/Desktop/out.txt", "w"); // Open "out.txt" on the local file system for writing
money24617111 1:b222972c5930 71
money24617111 1:b222972c5930 72
hoting 0:74ea99c4db88 73 init_TIMER();
hoting 0:74ea99c4db88 74 init_PWM();
hoting 0:74ea99c4db88 75 init_CN();
hoting 0:74ea99c4db88 76
hoting 0:74ea99c4db88 77 while(1) {
hoting 0:74ea99c4db88 78 }
hoting 0:74ea99c4db88 79 }
hoting 0:74ea99c4db88 80
hoting 0:74ea99c4db88 81 void init_TIMER()
hoting 0:74ea99c4db88 82 {
hoting 0:74ea99c4db88 83 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
hoting 0:74ea99c4db88 84 }
hoting 0:74ea99c4db88 85
hoting 0:74ea99c4db88 86 void init_PWM()
hoting 0:74ea99c4db88 87 {
hoting 0:74ea99c4db88 88 servo_cmd.period_ms(20);
hoting 0:74ea99c4db88 89 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 90
hoting 0:74ea99c4db88 91 pwm1.period_us(50);
money24617111 1:b222972c5930 92 pwm1.write(0.5f);
hoting 0:74ea99c4db88 93 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 94
hoting 0:74ea99c4db88 95 pwm2.period_us(50);
money24617111 1:b222972c5930 96 pwm2.write(0.5f);
hoting 0:74ea99c4db88 97 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 98 }
hoting 0:74ea99c4db88 99 void init_CN()
hoting 0:74ea99c4db88 100 {
hoting 0:74ea99c4db88 101 HallA.rise(&CN_ITR);
hoting 0:74ea99c4db88 102 HallA.fall(&CN_ITR);
hoting 0:74ea99c4db88 103 HallB.rise(&CN_ITR);
hoting 0:74ea99c4db88 104 HallB.fall(&CN_ITR);
hoting 0:74ea99c4db88 105
hoting 0:74ea99c4db88 106 HallA_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 107 HallA_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 108 HallB_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 109 HallB_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 110 }
hoting 0:74ea99c4db88 111
hoting 0:74ea99c4db88 112 void CN_ITR()
money24617111 1:b222972c5930 113 { // motor1
hoting 0:74ea99c4db88 114 HallA_1_state = HallA.read();
hoting 0:74ea99c4db88 115 HallB_1_state = HallB.read();
money24617111 1:b222972c5930 116
money24617111 1:b222972c5930 117 // motor2
money24617111 1:b222972c5930 118 HallA_2_state = HallA_2.read();
money24617111 1:b222972c5930 119 HallB_2_state = HallB_2.read();
hoting 0:74ea99c4db88 120
hoting 0:74ea99c4db88 121 ///code for state determination///
money24617111 1:b222972c5930 122
money24617111 1:b222972c5930 123 // state determine (hall1) //
money24617111 1:b222972c5930 124
money24617111 1:b222972c5930 125 if(HallA_1_state==0 && HallB_1_state ==0)
money24617111 1:b222972c5930 126 {
money24617111 1:b222972c5930 127 state_1 = 0;
money24617111 1:b222972c5930 128
money24617111 1:b222972c5930 129 }
money24617111 1:b222972c5930 130
money24617111 1:b222972c5930 131 else if(HallA_1_state==1 && HallB_1_state ==0)
money24617111 1:b222972c5930 132 {
money24617111 1:b222972c5930 133 state_1 = 1;
money24617111 1:b222972c5930 134 }
money24617111 1:b222972c5930 135
money24617111 1:b222972c5930 136 else if(HallA_1_state==0 && HallB_1_state ==1)
money24617111 1:b222972c5930 137 {
money24617111 1:b222972c5930 138 state_1 = 2;
money24617111 1:b222972c5930 139 }
money24617111 1:b222972c5930 140
money24617111 1:b222972c5930 141 else if(HallA_1_state==1 && HallB_1_state ==1)
money24617111 1:b222972c5930 142 {
money24617111 1:b222972c5930 143 state_1 = 3;
money24617111 1:b222972c5930 144 }
money24617111 1:b222972c5930 145 // end state determine of 1 //
money24617111 1:b222972c5930 146
money24617111 1:b222972c5930 147 // foward and backward determine of 1//
money24617111 1:b222972c5930 148
money24617111 1:b222972c5930 149 switch(state_1)
money24617111 1:b222972c5930 150 {
money24617111 1:b222972c5930 151 case 0 :
money24617111 1:b222972c5930 152
money24617111 1:b222972c5930 153 if(state_1_old == 1)
money24617111 1:b222972c5930 154 {
money24617111 1:b222972c5930 155 speed_count_1 ++; // foward
money24617111 1:b222972c5930 156 }
money24617111 1:b222972c5930 157
money24617111 1:b222972c5930 158 else if(state_1_old == 2)
money24617111 1:b222972c5930 159 {
money24617111 1:b222972c5930 160 speed_count_1 --; // reverse
money24617111 1:b222972c5930 161 }
money24617111 1:b222972c5930 162 else{}
money24617111 1:b222972c5930 163
money24617111 1:b222972c5930 164 break;
money24617111 1:b222972c5930 165 case 1 :
money24617111 1:b222972c5930 166
money24617111 1:b222972c5930 167 if(state_1_old == 3)
money24617111 1:b222972c5930 168 {
money24617111 1:b222972c5930 169 speed_count_1 ++; // foward
money24617111 1:b222972c5930 170 }
money24617111 1:b222972c5930 171
money24617111 1:b222972c5930 172 else if(state_1_old == 0)
money24617111 1:b222972c5930 173 {
money24617111 1:b222972c5930 174 speed_count_1 --; // reverse
money24617111 1:b222972c5930 175 }
money24617111 1:b222972c5930 176
money24617111 1:b222972c5930 177 else{}
money24617111 1:b222972c5930 178
money24617111 1:b222972c5930 179 break;
money24617111 1:b222972c5930 180 case 2 :
money24617111 1:b222972c5930 181
money24617111 1:b222972c5930 182 if(state_1_old == 0)
money24617111 1:b222972c5930 183 {
money24617111 1:b222972c5930 184 speed_count_1 ++; // foward
money24617111 1:b222972c5930 185 }
money24617111 1:b222972c5930 186
money24617111 1:b222972c5930 187 else if(state_1_old == 3)
money24617111 1:b222972c5930 188 {
money24617111 1:b222972c5930 189 speed_count_1 --; // reverse
money24617111 1:b222972c5930 190 }
money24617111 1:b222972c5930 191
money24617111 1:b222972c5930 192 else{}
money24617111 1:b222972c5930 193
money24617111 1:b222972c5930 194 break;
money24617111 1:b222972c5930 195 case 3 :
money24617111 1:b222972c5930 196
money24617111 1:b222972c5930 197 if(state_1_old == 2)
money24617111 1:b222972c5930 198 {
money24617111 1:b222972c5930 199 speed_count_1 ++; // foward
money24617111 1:b222972c5930 200 }
money24617111 1:b222972c5930 201
money24617111 1:b222972c5930 202 else if(state_1_old == 1)
money24617111 1:b222972c5930 203 {
money24617111 1:b222972c5930 204 speed_count_1 --; // reverse
money24617111 1:b222972c5930 205 }
money24617111 1:b222972c5930 206
money24617111 1:b222972c5930 207 else{}
money24617111 1:b222972c5930 208
money24617111 1:b222972c5930 209 break;
money24617111 1:b222972c5930 210
money24617111 1:b222972c5930 211 }
money24617111 1:b222972c5930 212
money24617111 1:b222972c5930 213
money24617111 1:b222972c5930 214
money24617111 1:b222972c5930 215 // end foward and reverse determine of 1 //
money24617111 1:b222972c5930 216
money24617111 1:b222972c5930 217 // update//
money24617111 1:b222972c5930 218 state_1_old = state_1;
money24617111 1:b222972c5930 219 // end update //
money24617111 1:b222972c5930 220
money24617111 1:b222972c5930 221
money24617111 1:b222972c5930 222
money24617111 1:b222972c5930 223
money24617111 1:b222972c5930 224 // state determine (hall2) //
money24617111 1:b222972c5930 225
money24617111 1:b222972c5930 226
money24617111 1:b222972c5930 227 if(HallA_2_state==0 && HallB_2_state ==0)
money24617111 1:b222972c5930 228 {
money24617111 1:b222972c5930 229 state_2 = 0;
money24617111 1:b222972c5930 230
money24617111 1:b222972c5930 231 }
money24617111 1:b222972c5930 232
money24617111 1:b222972c5930 233 else if(HallA_2_state==1 && HallB_2_state ==0)
money24617111 1:b222972c5930 234 {
money24617111 1:b222972c5930 235 state_2 = 1;
money24617111 1:b222972c5930 236 }
money24617111 1:b222972c5930 237
money24617111 1:b222972c5930 238 else if(HallA_2_state==0 && HallB_2_state ==1)
money24617111 1:b222972c5930 239 {
money24617111 1:b222972c5930 240 state_2 = 2;
money24617111 1:b222972c5930 241 }
money24617111 1:b222972c5930 242
money24617111 1:b222972c5930 243 else if(HallA_2_state==1 && HallB_2_state ==1)
money24617111 1:b222972c5930 244 {
money24617111 1:b222972c5930 245 state_2 = 3;
money24617111 1:b222972c5930 246 }
money24617111 1:b222972c5930 247 // end state determine of 2//
money24617111 1:b222972c5930 248
money24617111 1:b222972c5930 249 // foward and reverse determine of 2//
money24617111 1:b222972c5930 250
money24617111 1:b222972c5930 251 switch(state_2)
money24617111 1:b222972c5930 252 {
money24617111 1:b222972c5930 253 case 0 :
money24617111 1:b222972c5930 254
money24617111 1:b222972c5930 255 if(state_2_old == 1)
money24617111 1:b222972c5930 256 {
money24617111 1:b222972c5930 257 speed_count_2 ++; // foward
money24617111 1:b222972c5930 258 }
money24617111 1:b222972c5930 259
money24617111 1:b222972c5930 260 else if(state_2_old == 2)
money24617111 1:b222972c5930 261 {
money24617111 1:b222972c5930 262 speed_count_2 --; // reverse
money24617111 1:b222972c5930 263 }
money24617111 1:b222972c5930 264 else{}
money24617111 1:b222972c5930 265
money24617111 1:b222972c5930 266 break;
money24617111 1:b222972c5930 267 case 1 :
money24617111 1:b222972c5930 268
money24617111 1:b222972c5930 269 if(state_2_old == 3)
money24617111 1:b222972c5930 270 {
money24617111 1:b222972c5930 271 speed_count_2 ++; // foward
money24617111 1:b222972c5930 272 }
money24617111 1:b222972c5930 273
money24617111 1:b222972c5930 274 else if(state_2_old == 0)
money24617111 1:b222972c5930 275 {
money24617111 1:b222972c5930 276 speed_count_2 --; // reverse
money24617111 1:b222972c5930 277 }
money24617111 1:b222972c5930 278
money24617111 1:b222972c5930 279 else{}
money24617111 1:b222972c5930 280
money24617111 1:b222972c5930 281 break;
money24617111 1:b222972c5930 282 case 2 :
money24617111 1:b222972c5930 283
money24617111 1:b222972c5930 284 if(state_2_old == 0)
money24617111 1:b222972c5930 285 {
money24617111 1:b222972c5930 286 speed_count_2 ++; // foward
money24617111 1:b222972c5930 287 }
money24617111 1:b222972c5930 288
money24617111 1:b222972c5930 289 else if(state_2_old == 3)
money24617111 1:b222972c5930 290 {
money24617111 1:b222972c5930 291 speed_count_2 --; // reverse
money24617111 1:b222972c5930 292 }
money24617111 1:b222972c5930 293
money24617111 1:b222972c5930 294 else{}
money24617111 1:b222972c5930 295
money24617111 1:b222972c5930 296 break;
money24617111 1:b222972c5930 297 case 3 :
money24617111 1:b222972c5930 298
money24617111 1:b222972c5930 299 if(state_2_old == 2)
money24617111 1:b222972c5930 300 {
money24617111 1:b222972c5930 301 speed_count_2 ++; // foward
money24617111 1:b222972c5930 302 }
money24617111 1:b222972c5930 303
money24617111 1:b222972c5930 304 else if(state_2_old == 1)
money24617111 1:b222972c5930 305 {
money24617111 1:b222972c5930 306 speed_count_2 --; // reverse
money24617111 1:b222972c5930 307 }
money24617111 1:b222972c5930 308
money24617111 1:b222972c5930 309 else{}
money24617111 1:b222972c5930 310
money24617111 1:b222972c5930 311 break;
money24617111 1:b222972c5930 312
money24617111 1:b222972c5930 313 }
money24617111 1:b222972c5930 314
money24617111 1:b222972c5930 315
money24617111 1:b222972c5930 316
money24617111 1:b222972c5930 317 // end foward and backward determine of 1 //
money24617111 1:b222972c5930 318
money24617111 1:b222972c5930 319 // update//
money24617111 1:b222972c5930 320 state_2_old = state_2;
money24617111 1:b222972c5930 321 // end update //
money24617111 1:b222972c5930 322
money24617111 1:b222972c5930 323
money24617111 1:b222972c5930 324
hoting 0:74ea99c4db88 325
hoting 0:74ea99c4db88 326
hoting 0:74ea99c4db88 327 //////////////////////////////////
hoting 0:74ea99c4db88 328
hoting 0:74ea99c4db88 329 //forward : speed_count_1 + 1
hoting 0:74ea99c4db88 330 //backward : speed_count_1 - 1
hoting 0:74ea99c4db88 331 //forward : speed_count_2 + 1
hoting 0:74ea99c4db88 332 //backward : speed_count_2 - 1
money24617111 1:b222972c5930 333
hoting 0:74ea99c4db88 334 }
hoting 0:74ea99c4db88 335
hoting 0:74ea99c4db88 336 void timer1_ITR()
hoting 0:74ea99c4db88 337 {
hoting 0:74ea99c4db88 338 // servo motor
hoting 0:74ea99c4db88 339 ///code for sevo motor///
hoting 0:74ea99c4db88 340
hoting 0:74ea99c4db88 341
hoting 0:74ea99c4db88 342
hoting 0:74ea99c4db88 343 /////////////////////////
hoting 0:74ea99c4db88 344
hoting 0:74ea99c4db88 345 if(servo_duty >= 0.113f)servo_duty = 0.113;
hoting 0:74ea99c4db88 346 else if(servo_duty <= 0.025f)servo_duty = 0.025;
hoting 0:74ea99c4db88 347 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 348
hoting 0:74ea99c4db88 349 // motor1
money24617111 1:b222972c5930 350 rotation_speed_1 = speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
money24617111 1:b222972c5930 351 pc.printf("%d\r\n",speed_count_1);
money24617111 1:b222972c5930 352
hoting 0:74ea99c4db88 353 speed_count_1 = 0;
hoting 0:74ea99c4db88 354
hoting 0:74ea99c4db88 355 ///PI controller for motor1///
money24617111 1:b222972c5930 356 err_1 = Kp *(rotation_speed_ref_1 - rotation_speed_1);
money24617111 1:b222972c5930 357 ierr_1 = ierr_1 + Ki*(rotation_speed_ref_1- rotation_speed_1)*0.01;
hoting 0:74ea99c4db88 358
money24617111 1:b222972c5930 359 PI_out_1 = (err_1 + ierr_1)*0.002;
hoting 0:74ea99c4db88 360
hoting 0:74ea99c4db88 361
hoting 0:74ea99c4db88 362 //////////////////////////////
hoting 0:74ea99c4db88 363
hoting 0:74ea99c4db88 364 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
hoting 0:74ea99c4db88 365 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
hoting 0:74ea99c4db88 366 pwm1_duty = PI_out_1 + 0.5f;
hoting 0:74ea99c4db88 367 pwm1.write(pwm1_duty);
hoting 0:74ea99c4db88 368 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 369
hoting 0:74ea99c4db88 370 //motor2
hoting 0:74ea99c4db88 371 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
money24617111 1:b222972c5930 372
money24617111 1:b222972c5930 373
hoting 0:74ea99c4db88 374 speed_count_2 = 0;
hoting 0:74ea99c4db88 375
hoting 0:74ea99c4db88 376 ///PI controller for motor2///
money24617111 1:b222972c5930 377 err_2 = float(Kp *(rotation_speed_ref_2 - rotation_speed_2));
money24617111 1:b222972c5930 378 ierr_2 = float(ierr_1 + Ki*(rotation_speed_ref_2- rotation_speed_2)*0.01);
hoting 0:74ea99c4db88 379
money24617111 1:b222972c5930 380 PI_out_2 = float((rotation_speed_2+err_2 + ierr_2)*0.002);
hoting 0:74ea99c4db88 381
hoting 0:74ea99c4db88 382
hoting 0:74ea99c4db88 383 //////////////////////////////
hoting 0:74ea99c4db88 384
hoting 0:74ea99c4db88 385 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
hoting 0:74ea99c4db88 386 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
hoting 0:74ea99c4db88 387 pwm2_duty = PI_out_2 + 0.5f;
hoting 0:74ea99c4db88 388 pwm2.write(pwm2_duty);
hoting 0:74ea99c4db88 389 TIM1->CCER |= 0x40;
money24617111 1:b222972c5930 390
money24617111 1:b222972c5930 391
money24617111 1:b222972c5930 392
hoting 0:74ea99c4db88 393 }