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@2:c95a3cba51e6, 2017-03-23 (annotated)

Committer:
YutingHsiao
Date:
Thu Mar 23 06:36:33 2017 +0000
Revision:
2:c95a3cba51e6
Parent:
1:657bebc0fe37 
20170322

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