Robotics ^___^ / Mbed 2 deprecated Robotics_LAB_motor_Done

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@1:657bebc0fe37, 2017-03-21 (annotated)

Committer:
YutingHsiao
Date:
Tue Mar 21 14:32:42 2017 +0000
Revision:
1:657bebc0fe37
Parent:
0:74ea99c4db88
Child:
2:c95a3cba51e6
qqqqqq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hoting 0:74ea99c4db88 1 #include "mbed.h"
hoting 0:74ea99c4db88 2
hoting 0:74ea99c4db88 3 //The number will be compiled as type "double" in default
hoting 0:74ea99c4db88 4 //Add a "f" after the number can make it compiled as type "float"
hoting 0:74ea99c4db88 5 #define Ts 0.01f //period of timer1 (s)
hoting 0:74ea99c4db88 6
YutingHsiao 1:657bebc0fe37 7 Serial pc(USBTX, USBRX);
YutingHsiao 1:657bebc0fe37 8
hoting 0:74ea99c4db88 9 Ticker timer1;
hoting 0:74ea99c4db88 10 // servo motor
hoting 0:74ea99c4db88 11 PwmOut servo_cmd(A0);
hoting 0:74ea99c4db88 12 // DC motor
hoting 0:74ea99c4db88 13 PwmOut pwm1(D7);
hoting 0:74ea99c4db88 14 PwmOut pwm1n(D11);
hoting 0:74ea99c4db88 15 PwmOut pwm2(D8);
hoting 0:74ea99c4db88 16 PwmOut pwm2n(A3);
hoting 0:74ea99c4db88 17
hoting 0:74ea99c4db88 18 // Motor1 sensor
hoting 0:74ea99c4db88 19 InterruptIn HallA(A1);
hoting 0:74ea99c4db88 20 InterruptIn HallB(A2);
hoting 0:74ea99c4db88 21 // Motor2 sensor
hoting 0:74ea99c4db88 22 InterruptIn HallA_2(D13);
hoting 0:74ea99c4db88 23 InterruptIn HallB_2(D12);
hoting 0:74ea99c4db88 24
YutingHsiao 1:657bebc0fe37 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;
YutingHsiao 1:657bebc0fe37 52 float rotation_speed_ref_1 = 150.0;//150rpm
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;
YutingHsiao 1:657bebc0fe37 59 float rotation_speed_ref_2 = -80.0;//-80rpm
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
YutingHsiao 1:657bebc0fe37 65 //set parameters
YutingHsiao 1:657bebc0fe37 66 float kp = 0.4;
YutingHsiao 1:657bebc0fe37 67 float ki = 0.01;//0.0x
YutingHsiao 1:657bebc0fe37 68
hoting 0:74ea99c4db88 69 int main()
hoting 0:74ea99c4db88 70 {
YutingHsiao 1:657bebc0fe37 71 //pc.printf("hello main()\n");
hoting 0:74ea99c4db88 72 init_TIMER();
hoting 0:74ea99c4db88 73 init_PWM();
hoting 0:74ea99c4db88 74 init_CN();
hoting 0:74ea99c4db88 75
hoting 0:74ea99c4db88 76 while(1) {
YutingHsiao 1:657bebc0fe37 77 //pc.printf("state_2 = %d \n", state_2);
YutingHsiao 1:657bebc0fe37 78 pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
YutingHsiao 1:657bebc0fe37 79 pc.printf("rotation_speed_1 = %f \n", rotation_speed_1);
hoting 0:74ea99c4db88 80 }
hoting 0:74ea99c4db88 81 }
hoting 0:74ea99c4db88 82
hoting 0:74ea99c4db88 83 void init_TIMER()
hoting 0:74ea99c4db88 84 {
YutingHsiao 1:657bebc0fe37 85 //pc.printf("hello TIMER()\n");
hoting 0:74ea99c4db88 86 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 87 }
hoting 0:74ea99c4db88 88
hoting 0:74ea99c4db88 89 void init_PWM()
hoting 0:74ea99c4db88 90 {
hoting 0:74ea99c4db88 91 servo_cmd.period_ms(20);
hoting 0:74ea99c4db88 92 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 93
hoting 0:74ea99c4db88 94 pwm1.period_us(50);
hoting 0:74ea99c4db88 95 pwm1.write(0.5);
hoting 0:74ea99c4db88 96 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 97
hoting 0:74ea99c4db88 98 pwm2.period_us(50);
hoting 0:74ea99c4db88 99 pwm2.write(0.5);
hoting 0:74ea99c4db88 100 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 101 }
hoting 0:74ea99c4db88 102 void init_CN()
hoting 0:74ea99c4db88 103 {
YutingHsiao 1:657bebc0fe37 104 //pc.printf("hello init_CN \n");
hoting 0:74ea99c4db88 105 HallA.rise(&CN_ITR);
hoting 0:74ea99c4db88 106 HallA.fall(&CN_ITR);
hoting 0:74ea99c4db88 107 HallB.rise(&CN_ITR);
hoting 0:74ea99c4db88 108 HallB.fall(&CN_ITR);
hoting 0:74ea99c4db88 109
hoting 0:74ea99c4db88 110 HallA_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 111 HallA_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 112 HallB_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 113 HallB_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 114 }
hoting 0:74ea99c4db88 115
hoting 0:74ea99c4db88 116 void CN_ITR()
hoting 0:74ea99c4db88 117 {
hoting 0:74ea99c4db88 118 // motor1
hoting 0:74ea99c4db88 119 HallA_1_state = HallA.read();
hoting 0:74ea99c4db88 120 HallB_1_state = HallB.read();
hoting 0:74ea99c4db88 121
hoting 0:74ea99c4db88 122 ///code for state determination///
YutingHsiao 1:657bebc0fe37 123 //////////////////////////////////
hoting 0:74ea99c4db88 124 //////////////////////////////////
YutingHsiao 1:657bebc0fe37 125 //determine the state
YutingHsiao 1:657bebc0fe37 126 if((HallA_1_state == 0)&&(HallB_1_state == 0))
YutingHsiao 1:657bebc0fe37 127 {
YutingHsiao 1:657bebc0fe37 128 state_1 = 1;
YutingHsiao 1:657bebc0fe37 129 }
YutingHsiao 1:657bebc0fe37 130 else if((HallA_1_state == 0)&&(HallB_1_state == 1))
YutingHsiao 1:657bebc0fe37 131 {
YutingHsiao 1:657bebc0fe37 132 state_1 = 2;
YutingHsiao 1:657bebc0fe37 133 }
YutingHsiao 1:657bebc0fe37 134 else if((HallA_1_state == 1)&&(HallB_1_state == 1))
YutingHsiao 1:657bebc0fe37 135 {
YutingHsiao 1:657bebc0fe37 136 state_1 = 3;
YutingHsiao 1:657bebc0fe37 137 }
YutingHsiao 1:657bebc0fe37 138 else if((HallA_1_state == 1)&&(HallB_1_state ==0))
YutingHsiao 1:657bebc0fe37 139 {
YutingHsiao 1:657bebc0fe37 140 state_1 = 4;
YutingHsiao 1:657bebc0fe37 141 }
YutingHsiao 1:657bebc0fe37 142
YutingHsiao 1:657bebc0fe37 143 //forward or backward
YutingHsiao 1:657bebc0fe37 144 int direction_1 = 0;
YutingHsiao 1:657bebc0fe37 145 direction_1 = state_1 - state_1_old;
YutingHsiao 1:657bebc0fe37 146 if((direction_1 == -1) || (direction_1 == 3))
YutingHsiao 1:657bebc0fe37 147 {
YutingHsiao 1:657bebc0fe37 148 //backward
YutingHsiao 1:657bebc0fe37 149 speed_count_1 = speed_count_1 - 1;
YutingHsiao 1:657bebc0fe37 150 }
YutingHsiao 1:657bebc0fe37 151 else if((direction_1 == 1) || (direction_1 == -3))
YutingHsiao 1:657bebc0fe37 152 {
YutingHsiao 1:657bebc0fe37 153 //forward
YutingHsiao 1:657bebc0fe37 154 speed_count_1 = speed_count_1 + 1;
YutingHsiao 1:657bebc0fe37 155 }
YutingHsiao 1:657bebc0fe37 156 else
YutingHsiao 1:657bebc0fe37 157 {
YutingHsiao 1:657bebc0fe37 158 //prevent initializing error
YutingHsiao 1:657bebc0fe37 159 state_1_old = state_1;
YutingHsiao 1:657bebc0fe37 160 }
YutingHsiao 1:657bebc0fe37 161
YutingHsiao 1:657bebc0fe37 162 //change old state
YutingHsiao 1:657bebc0fe37 163 state_1_old = state_1;
YutingHsiao 1:657bebc0fe37 164 //////////////////////////////////
YutingHsiao 1:657bebc0fe37 165 //////////////////////////////////
hoting 0:74ea99c4db88 166 //forward : speed_count_1 + 1
hoting 0:74ea99c4db88 167 //backward : speed_count_1 - 1
hoting 0:74ea99c4db88 168
hoting 0:74ea99c4db88 169 // motor2
hoting 0:74ea99c4db88 170 HallA_2_state = HallA_2.read();
hoting 0:74ea99c4db88 171 HallB_2_state = HallB_2.read();
hoting 0:74ea99c4db88 172
hoting 0:74ea99c4db88 173 ///code for state determination///
hoting 0:74ea99c4db88 174 //////////////////////////////////
YutingHsiao 1:657bebc0fe37 175 /////////////////////////////////
YutingHsiao 1:657bebc0fe37 176 //determine the state
YutingHsiao 1:657bebc0fe37 177 if((HallA_2_state == 0)&&(HallB_2_state == 0))
YutingHsiao 1:657bebc0fe37 178 {
YutingHsiao 1:657bebc0fe37 179 state_2 = 1;
YutingHsiao 1:657bebc0fe37 180 }
YutingHsiao 1:657bebc0fe37 181 else if((HallA_2_state == 0)&&(HallB_2_state == 1))
YutingHsiao 1:657bebc0fe37 182 {
YutingHsiao 1:657bebc0fe37 183 state_2 = 2;
YutingHsiao 1:657bebc0fe37 184 }
YutingHsiao 1:657bebc0fe37 185 else if((HallA_2_state == 1)&&(HallB_2_state == 1))
YutingHsiao 1:657bebc0fe37 186 {
YutingHsiao 1:657bebc0fe37 187 state_2 = 3;
YutingHsiao 1:657bebc0fe37 188 }
YutingHsiao 1:657bebc0fe37 189 else if((HallA_2_state == 1)&&(HallB_2_state ==0))
YutingHsiao 1:657bebc0fe37 190 {
YutingHsiao 1:657bebc0fe37 191 state_2 = 4;
YutingHsiao 1:657bebc0fe37 192 }
YutingHsiao 1:657bebc0fe37 193
YutingHsiao 1:657bebc0fe37 194 //forward or backward
YutingHsiao 1:657bebc0fe37 195 int direction_2 = 0;
YutingHsiao 1:657bebc0fe37 196 direction_2 = state_2 - state_2_old;
YutingHsiao 1:657bebc0fe37 197 if((direction_2 == 1) || (direction_2 == -3))
YutingHsiao 1:657bebc0fe37 198 {
YutingHsiao 1:657bebc0fe37 199 //backward
YutingHsiao 1:657bebc0fe37 200 speed_count_2 = speed_count_2 - 1;
YutingHsiao 1:657bebc0fe37 201 }
YutingHsiao 1:657bebc0fe37 202 else if((direction_2 == -1) || (direction_2 == 3))
YutingHsiao 1:657bebc0fe37 203 {
YutingHsiao 1:657bebc0fe37 204 //forward
YutingHsiao 1:657bebc0fe37 205 speed_count_2 = speed_count_2 + 1;
YutingHsiao 1:657bebc0fe37 206 }
YutingHsiao 1:657bebc0fe37 207 else
YutingHsiao 1:657bebc0fe37 208 {
YutingHsiao 1:657bebc0fe37 209 //prevent initializing error
YutingHsiao 1:657bebc0fe37 210 state_2_old = state_2;
YutingHsiao 1:657bebc0fe37 211 }
YutingHsiao 1:657bebc0fe37 212
YutingHsiao 1:657bebc0fe37 213 //change old state
YutingHsiao 1:657bebc0fe37 214 state_2_old = state_2;
YutingHsiao 1:657bebc0fe37 215 //////////////////////////////////
YutingHsiao 1:657bebc0fe37 216 //////////////////////////////////
hoting 0:74ea99c4db88 217 //forward : speed_count_2 + 1
hoting 0:74ea99c4db88 218 //backward : speed_count_2 - 1
hoting 0:74ea99c4db88 219 }
hoting 0:74ea99c4db88 220
hoting 0:74ea99c4db88 221 void timer1_ITR()
hoting 0:74ea99c4db88 222 {
YutingHsiao 1:657bebc0fe37 223 //pc.printf("hello timer1_ITR\n");
hoting 0:74ea99c4db88 224 // servo motor
hoting 0:74ea99c4db88 225 ///code for sevo motor///
YutingHsiao 1:657bebc0fe37 226 /////////////////////////
YutingHsiao 1:657bebc0fe37 227 /////////////////////////
YutingHsiao 1:657bebc0fe37 228 //rotating angle for every call
YutingHsiao 1:657bebc0fe37 229 angle = 15;
hoting 0:74ea99c4db88 230
YutingHsiao 1:657bebc0fe37 231 //servo_duty output for every call
YutingHsiao 1:657bebc0fe37 232 servo_duty = servo_duty + 0.088/180*angle/100;
YutingHsiao 1:657bebc0fe37 233 /////////////////////////
hoting 0:74ea99c4db88 234 /////////////////////////
hoting 0:74ea99c4db88 235
YutingHsiao 1:657bebc0fe37 236 //protection code for servo
hoting 0:74ea99c4db88 237 if(servo_duty >= 0.113f)servo_duty = 0.113;
hoting 0:74ea99c4db88 238 else if(servo_duty <= 0.025f)servo_duty = 0.025;
hoting 0:74ea99c4db88 239 servo_cmd.write(servo_duty);
YutingHsiao 1:657bebc0fe37 240
hoting 0:74ea99c4db88 241 // motor1
hoting 0:74ea99c4db88 242 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
YutingHsiao 1:657bebc0fe37 243 //pc.printf("rotation_speed_1 = %d \n\r", rotation_speed_1);
hoting 0:74ea99c4db88 244 speed_count_1 = 0;
YutingHsiao 1:657bebc0fe37 245 //pc.printf("speed_count_1 = %d \n\r",speed_count_1);
hoting 0:74ea99c4db88 246
hoting 0:74ea99c4db88 247 ///PI controller for motor1///
YutingHsiao 1:657bebc0fe37 248 //////////////////////////////
YutingHsiao 1:657bebc0fe37 249 //////////////////////////////
hoting 0:74ea99c4db88 250
YutingHsiao 1:657bebc0fe37 251 //PI control
YutingHsiao 1:657bebc0fe37 252 err_1 = rotation_speed_ref_1 - rotation_speed_1;
YutingHsiao 1:657bebc0fe37 253 //pc.printf("err_1 = %d \n\r", err_1);
YutingHsiao 1:657bebc0fe37 254 ierr_1 += err_1*Ts;
YutingHsiao 1:657bebc0fe37 255 PI_out_1 = kp * err_1 + ki * ierr_1;
YutingHsiao 1:657bebc0fe37 256 //pc.printf("PI_out_1 = %d \n\r", PI_out_1);
hoting 0:74ea99c4db88 257
hoting 0:74ea99c4db88 258 //////////////////////////////
YutingHsiao 1:657bebc0fe37 259 //////////////////////////////
hoting 0:74ea99c4db88 260
hoting 0:74ea99c4db88 261 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
hoting 0:74ea99c4db88 262 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
hoting 0:74ea99c4db88 263 pwm1_duty = PI_out_1 + 0.5f;
YutingHsiao 1:657bebc0fe37 264 //pc.printf("pwm1_duty = %d \n\r", pwm1_duty);
hoting 0:74ea99c4db88 265 pwm1.write(pwm1_duty);
hoting 0:74ea99c4db88 266 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 267
hoting 0:74ea99c4db88 268 //motor2
YutingHsiao 1:657bebc0fe37 269 //pc.printf("rotation_speed_ref_2 = %f \n",rotation_speed_ref_2 );
hoting 0:74ea99c4db88 270 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
YutingHsiao 1:657bebc0fe37 271 //pc.printf("rotation_speed_2 = %f \n", rotation_speed_2);
hoting 0:74ea99c4db88 272 speed_count_2 = 0;
YutingHsiao 1:657bebc0fe37 273 //pc.printf("speed_count_2 = %d \n", speed_count_2);
hoting 0:74ea99c4db88 274
hoting 0:74ea99c4db88 275 ///PI controller for motor2///
YutingHsiao 1:657bebc0fe37 276 //////////////////////////////
YutingHsiao 1:657bebc0fe37 277 //////////////////////////////
hoting 0:74ea99c4db88 278
YutingHsiao 1:657bebc0fe37 279 //PI control
YutingHsiao 1:657bebc0fe37 280
YutingHsiao 1:657bebc0fe37 281 err_2 = rotation_speed_ref_2 - rotation_speed_2;
YutingHsiao 1:657bebc0fe37 282 //pc.printf("err_2 = %f \n\r", err_2);
YutingHsiao 1:657bebc0fe37 283 ierr_2 += err_2 * Ts;
YutingHsiao 1:657bebc0fe37 284 PI_out_2 = kp * err_2 + ki * ierr_2;
YutingHsiao 1:657bebc0fe37 285 //pc.printf("PI_out_2 = %d \n\r", PI_out_2);
hoting 0:74ea99c4db88 286
hoting 0:74ea99c4db88 287 //////////////////////////////
YutingHsiao 1:657bebc0fe37 288 //////////////////////////////
hoting 0:74ea99c4db88 289
hoting 0:74ea99c4db88 290 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
hoting 0:74ea99c4db88 291 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
hoting 0:74ea99c4db88 292 pwm2_duty = PI_out_2 + 0.5f;
YutingHsiao 1:657bebc0fe37 293 //pc.printf("pwm2_duty = %d \n\r", pwm2_duty);
hoting 0:74ea99c4db88 294 pwm2.write(pwm2_duty);
hoting 0:74ea99c4db88 295 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 296 }