ROBOTIC CHAMPION TEAM / Mbed 2 deprecated Robotics_DC_SERVO_Motor Featured

1

Dependencies:   mbed

main.cpp@0:d7739916cfb1, 2017-03-23 (annotated)

Committer:
hsuan2481
Date:
Thu Mar 23 06:33:10 2017 +0000
Revision:
0:d7739916cfb1
ROBOTIC CHAMPION TEAM

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hsuan2481 0:d7739916cfb1 1 #include "mbed.h"
hsuan2481 0:d7739916cfb1 2
hsuan2481 0:d7739916cfb1 3 //The number will be compiled as type "double" in default
hsuan2481 0:d7739916cfb1 4 //Add a "f" after the number can make it compiled as type "float"
hsuan2481 0:d7739916cfb1 5 #define Ts 0.01f //period of timer1 (s)
hsuan2481 0:d7739916cfb1 6 #define Kp 0.006f
hsuan2481 0:d7739916cfb1 7 #define Ki 0.02f
hsuan2481 0:d7739916cfb1 8
hsuan2481 0:d7739916cfb1 9 Ticker timer1;
hsuan2481 0:d7739916cfb1 10 // servo motor
hsuan2481 0:d7739916cfb1 11 PwmOut servo_cmd(A0);
hsuan2481 0:d7739916cfb1 12 // DC motor
hsuan2481 0:d7739916cfb1 13 PwmOut pwm1(D7);
hsuan2481 0:d7739916cfb1 14 PwmOut pwm1n(D11);
hsuan2481 0:d7739916cfb1 15 PwmOut pwm2(D8);
hsuan2481 0:d7739916cfb1 16 PwmOut pwm2n(A3);
hsuan2481 0:d7739916cfb1 17
hsuan2481 0:d7739916cfb1 18 // Motor1 sensor
hsuan2481 0:d7739916cfb1 19 InterruptIn HallA(A1);
hsuan2481 0:d7739916cfb1 20 InterruptIn HallB(A2);
hsuan2481 0:d7739916cfb1 21 // Motor2 sensor
hsuan2481 0:d7739916cfb1 22 InterruptIn HallA_2(D13);
hsuan2481 0:d7739916cfb1 23 InterruptIn HallB_2(D12);
hsuan2481 0:d7739916cfb1 24
hsuan2481 0:d7739916cfb1 25 // 函式宣告
hsuan2481 0:d7739916cfb1 26 void init_IO();
hsuan2481 0:d7739916cfb1 27 void init_TIMER();
hsuan2481 0:d7739916cfb1 28 void timer1_ITR();
hsuan2481 0:d7739916cfb1 29 void init_CN();
hsuan2481 0:d7739916cfb1 30 void CN_ITR();
hsuan2481 0:d7739916cfb1 31 void init_PWM();
hsuan2481 0:d7739916cfb1 32
hsuan2481 0:d7739916cfb1 33 // servo motor
hsuan2481 0:d7739916cfb1 34 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
hsuan2481 0:d7739916cfb1 35 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
hsuan2481 0:d7739916cfb1 36 int angle = 0;
hsuan2481 0:d7739916cfb1 37 int counter;
hsuan2481 0:d7739916cfb1 38
hsuan2481 0:d7739916cfb1 39
hsuan2481 0:d7739916cfb1 40 // Hall sensor
hsuan2481 0:d7739916cfb1 41 int HallA_1_state = 0;
hsuan2481 0:d7739916cfb1 42 int HallB_1_state = 0;
hsuan2481 0:d7739916cfb1 43 int state_1 = 0;
hsuan2481 0:d7739916cfb1 44 int state_1_old = 0;
hsuan2481 0:d7739916cfb1 45 int HallA_2_state = 0;
hsuan2481 0:d7739916cfb1 46 int HallB_2_state = 0;
hsuan2481 0:d7739916cfb1 47 int state_2 = 0;
hsuan2481 0:d7739916cfb1 48 int state_2_old = 0;
hsuan2481 0:d7739916cfb1 49
hsuan2481 0:d7739916cfb1 50 int c = 0;
hsuan2481 0:d7739916cfb1 51 int d = 0;
hsuan2481 0:d7739916cfb1 52
hsuan2481 0:d7739916cfb1 53 // DC motor rotation speed control
hsuan2481 0:d7739916cfb1 54 int speed_count_1 = 0;
hsuan2481 0:d7739916cfb1 55 float rotation_speed_1 = 0.0;
hsuan2481 0:d7739916cfb1 56 float rotation_speed_ref_1 = 150;
hsuan2481 0:d7739916cfb1 57 float pwm1_duty = 0.5;
hsuan2481 0:d7739916cfb1 58 float PI_out_1 = 0.0;
hsuan2481 0:d7739916cfb1 59 float err_1 = 0.0;
hsuan2481 0:d7739916cfb1 60 float ierr_1 = 0.0;
hsuan2481 0:d7739916cfb1 61 int speed_count_2 = 0;
hsuan2481 0:d7739916cfb1 62 float rotation_speed_2 = 0.0;
hsuan2481 0:d7739916cfb1 63 float rotation_speed_ref_2 =80;
hsuan2481 0:d7739916cfb1 64 float pwm2_duty = 0.5;
hsuan2481 0:d7739916cfb1 65 float PI_out_2 = 0.0;
hsuan2481 0:d7739916cfb1 66 float err_2 = 0.0;
hsuan2481 0:d7739916cfb1 67 float ierr_2 = 0.0;
hsuan2481 0:d7739916cfb1 68
hsuan2481 0:d7739916cfb1 69 int main()
hsuan2481 0:d7739916cfb1 70 {
hsuan2481 0:d7739916cfb1 71 init_TIMER();
hsuan2481 0:d7739916cfb1 72 init_PWM();
hsuan2481 0:d7739916cfb1 73 init_CN();
hsuan2481 0:d7739916cfb1 74
hsuan2481 0:d7739916cfb1 75 while(1) {
hsuan2481 0:d7739916cfb1 76 }
hsuan2481 0:d7739916cfb1 77 }
hsuan2481 0:d7739916cfb1 78
hsuan2481 0:d7739916cfb1 79 void init_TIMER()
hsuan2481 0:d7739916cfb1 80 {
hsuan2481 0:d7739916cfb1 81 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
hsuan2481 0:d7739916cfb1 82 }
hsuan2481 0:d7739916cfb1 83
hsuan2481 0:d7739916cfb1 84 void init_PWM()
hsuan2481 0:d7739916cfb1 85 {
hsuan2481 0:d7739916cfb1 86 servo_cmd.period_ms(20);
hsuan2481 0:d7739916cfb1 87 servo_cmd.write(servo_duty);
hsuan2481 0:d7739916cfb1 88
hsuan2481 0:d7739916cfb1 89 pwm1.period_us(50);
hsuan2481 0:d7739916cfb1 90 pwm1.write(0.5);
hsuan2481 0:d7739916cfb1 91 TIM1->CCER |= 0x4;
hsuan2481 0:d7739916cfb1 92
hsuan2481 0:d7739916cfb1 93 pwm2.period_us(50);
hsuan2481 0:d7739916cfb1 94 pwm2.write(0.5);
hsuan2481 0:d7739916cfb1 95 TIM1->CCER |= 0x40;
hsuan2481 0:d7739916cfb1 96 }
hsuan2481 0:d7739916cfb1 97 void init_CN()
hsuan2481 0:d7739916cfb1 98 {
hsuan2481 0:d7739916cfb1 99 HallA.rise(&CN_ITR);
hsuan2481 0:d7739916cfb1 100 HallA.fall(&CN_ITR);
hsuan2481 0:d7739916cfb1 101 HallB.rise(&CN_ITR);
hsuan2481 0:d7739916cfb1 102 HallB.fall(&CN_ITR);
hsuan2481 0:d7739916cfb1 103
hsuan2481 0:d7739916cfb1 104 HallA_2.rise(&CN_ITR);
hsuan2481 0:d7739916cfb1 105 HallA_2.fall(&CN_ITR);
hsuan2481 0:d7739916cfb1 106 HallB_2.rise(&CN_ITR);
hsuan2481 0:d7739916cfb1 107 HallB_2.fall(&CN_ITR);
hsuan2481 0:d7739916cfb1 108 }
hsuan2481 0:d7739916cfb1 109
hsuan2481 0:d7739916cfb1 110 void CN_ITR()
hsuan2481 0:d7739916cfb1 111 {
hsuan2481 0:d7739916cfb1 112 // motor1
hsuan2481 0:d7739916cfb1 113 HallA_1_state = HallA.read();
hsuan2481 0:d7739916cfb1 114 HallB_1_state = HallB.read();
hsuan2481 0:d7739916cfb1 115
hsuan2481 0:d7739916cfb1 116 ///code for state determination///
hsuan2481 0:d7739916cfb1 117
hsuan2481 0:d7739916cfb1 118
hsuan2481 0:d7739916cfb1 119 if(HallA_1_state == state_1_old && c == 1)
hsuan2481 0:d7739916cfb1 120 {
hsuan2481 0:d7739916cfb1 121 speed_count_1 = speed_count_1 + 1;
hsuan2481 0:d7739916cfb1 122 }
hsuan2481 0:d7739916cfb1 123
hsuan2481 0:d7739916cfb1 124 if(HallB_1_state == state_1_old && c == 1)
hsuan2481 0:d7739916cfb1 125 {
hsuan2481 0:d7739916cfb1 126 speed_count_1 = speed_count_1 - 1;
hsuan2481 0:d7739916cfb1 127 }
hsuan2481 0:d7739916cfb1 128
hsuan2481 0:d7739916cfb1 129 c = 1;
hsuan2481 0:d7739916cfb1 130 state_1_old = HallA_1_state;
hsuan2481 0:d7739916cfb1 131 state_1_old = HallB_1_state;
hsuan2481 0:d7739916cfb1 132
hsuan2481 0:d7739916cfb1 133
hsuan2481 0:d7739916cfb1 134 //////////////////////////////////
hsuan2481 0:d7739916cfb1 135
hsuan2481 0:d7739916cfb1 136 //forward : speed_count_1 + 1
hsuan2481 0:d7739916cfb1 137 //backward : speed_count_1 - 1
hsuan2481 0:d7739916cfb1 138
hsuan2481 0:d7739916cfb1 139
hsuan2481 0:d7739916cfb1 140 // motor2
hsuan2481 0:d7739916cfb1 141 HallA_2_state = HallA_2.read();
hsuan2481 0:d7739916cfb1 142 HallB_2_state = HallB_2.read();
hsuan2481 0:d7739916cfb1 143
hsuan2481 0:d7739916cfb1 144 ///code for state determination///
hsuan2481 0:d7739916cfb1 145
hsuan2481 0:d7739916cfb1 146
hsuan2481 0:d7739916cfb1 147 if(HallA_2_state == state_2_old && d == 1)
hsuan2481 0:d7739916cfb1 148 {
hsuan2481 0:d7739916cfb1 149 speed_count_2 = speed_count_2 + 1;
hsuan2481 0:d7739916cfb1 150 }
hsuan2481 0:d7739916cfb1 151
hsuan2481 0:d7739916cfb1 152 if(HallB_2_state == state_2_old && d == 1)
hsuan2481 0:d7739916cfb1 153 {
hsuan2481 0:d7739916cfb1 154 speed_count_2 = speed_count_2 - 1;
hsuan2481 0:d7739916cfb1 155 }
hsuan2481 0:d7739916cfb1 156
hsuan2481 0:d7739916cfb1 157 d = 1;
hsuan2481 0:d7739916cfb1 158 state_2_old = HallA_2_state;
hsuan2481 0:d7739916cfb1 159 state_2_old = HallB_2_state;
hsuan2481 0:d7739916cfb1 160
hsuan2481 0:d7739916cfb1 161 //////////////////////////////////
hsuan2481 0:d7739916cfb1 162
hsuan2481 0:d7739916cfb1 163 //forward : speed_count_2 + 1
hsuan2481 0:d7739916cfb1 164 //backward : speed_count_2 - 1
hsuan2481 0:d7739916cfb1 165 }
hsuan2481 0:d7739916cfb1 166
hsuan2481 0:d7739916cfb1 167 void timer1_ITR()
hsuan2481 0:d7739916cfb1 168 {
hsuan2481 0:d7739916cfb1 169 // servo motor
hsuan2481 0:d7739916cfb1 170 ///code for sevo motor///
hsuan2481 0:d7739916cfb1 171
hsuan2481 0:d7739916cfb1 172 counter = counter + 1;
hsuan2481 0:d7739916cfb1 173
hsuan2481 0:d7739916cfb1 174 if(counter == 100)
hsuan2481 0:d7739916cfb1 175 {
hsuan2481 0:d7739916cfb1 176 servo_duty = 0.069;
hsuan2481 0:d7739916cfb1 177 }
hsuan2481 0:d7739916cfb1 178 if(counter == 200)
hsuan2481 0:d7739916cfb1 179 {
hsuan2481 0:d7739916cfb1 180 servo_duty = 0.0763;
hsuan2481 0:d7739916cfb1 181 }
hsuan2481 0:d7739916cfb1 182 if(counter == 300)
hsuan2481 0:d7739916cfb1 183 {
hsuan2481 0:d7739916cfb1 184 servo_duty = 0.0837;
hsuan2481 0:d7739916cfb1 185 }
hsuan2481 0:d7739916cfb1 186 if(counter == 400)
hsuan2481 0:d7739916cfb1 187 {
hsuan2481 0:d7739916cfb1 188 servo_duty = 0.091;
hsuan2481 0:d7739916cfb1 189 }
hsuan2481 0:d7739916cfb1 190 if(counter == 500)
hsuan2481 0:d7739916cfb1 191 {
hsuan2481 0:d7739916cfb1 192 servo_duty = 0.0983;
hsuan2481 0:d7739916cfb1 193 }
hsuan2481 0:d7739916cfb1 194 if(counter == 600)
hsuan2481 0:d7739916cfb1 195 {
hsuan2481 0:d7739916cfb1 196 servo_duty = 0.106;
hsuan2481 0:d7739916cfb1 197 }
hsuan2481 0:d7739916cfb1 198
hsuan2481 0:d7739916cfb1 199 if(counter == 700)
hsuan2481 0:d7739916cfb1 200 {
hsuan2481 0:d7739916cfb1 201 servo_duty = 0.113;
hsuan2481 0:d7739916cfb1 202 }
hsuan2481 0:d7739916cfb1 203 if(counter > 700)
hsuan2481 0:d7739916cfb1 204 {
hsuan2481 0:d7739916cfb1 205 counter=0;
hsuan2481 0:d7739916cfb1 206 }
hsuan2481 0:d7739916cfb1 207
hsuan2481 0:d7739916cfb1 208 servo_cmd.write(servo_duty);
hsuan2481 0:d7739916cfb1 209
hsuan2481 0:d7739916cfb1 210
hsuan2481 0:d7739916cfb1 211 /////////////////////////
hsuan2481 0:d7739916cfb1 212
hsuan2481 0:d7739916cfb1 213 if(servo_duty >= 0.113f)servo_duty = 0.113;
hsuan2481 0:d7739916cfb1 214 else if(servo_duty <= 0.025f)servo_duty = 0.025;
hsuan2481 0:d7739916cfb1 215 servo_cmd.write(servo_duty);
hsuan2481 0:d7739916cfb1 216
hsuan2481 0:d7739916cfb1 217 // motor1
hsuan2481 0:d7739916cfb1 218 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
hsuan2481 0:d7739916cfb1 219 speed_count_1 = 0;
hsuan2481 0:d7739916cfb1 220
hsuan2481 0:d7739916cfb1 221 ///PI controller for motor1///
hsuan2481 0:d7739916cfb1 222
hsuan2481 0:d7739916cfb1 223 err_1 = rotation_speed_ref_1 - rotation_speed_1;
hsuan2481 0:d7739916cfb1 224 ierr_1 = ierr_1 + err_1*Ts;
hsuan2481 0:d7739916cfb1 225 PI_out_1 = Kp*err_1 + Ki*ierr_1;
hsuan2481 0:d7739916cfb1 226
hsuan2481 0:d7739916cfb1 227 //////////////////////////////
hsuan2481 0:d7739916cfb1 228
hsuan2481 0:d7739916cfb1 229 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
hsuan2481 0:d7739916cfb1 230 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
hsuan2481 0:d7739916cfb1 231 pwm1_duty = PI_out_1 + 0.5f;
hsuan2481 0:d7739916cfb1 232 pwm1.write(PI_out_1 + 0.5f);
hsuan2481 0:d7739916cfb1 233 TIM1->CCER |= 0x4;
hsuan2481 0:d7739916cfb1 234
hsuan2481 0:d7739916cfb1 235 //motor2
hsuan2481 0:d7739916cfb1 236 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
hsuan2481 0:d7739916cfb1 237 speed_count_2 = 0;
hsuan2481 0:d7739916cfb1 238
hsuan2481 0:d7739916cfb1 239 ///PI controller for motor2///
hsuan2481 0:d7739916cfb1 240
hsuan2481 0:d7739916cfb1 241 err_2 = rotation_speed_ref_2 - rotation_speed_2;
hsuan2481 0:d7739916cfb1 242 ierr_2 = ierr_2 + err_2*Ts;
hsuan2481 0:d7739916cfb1 243 PI_out_2 = Kp*err_2 + Ki*ierr_2;
hsuan2481 0:d7739916cfb1 244
hsuan2481 0:d7739916cfb1 245 //////////////////////////////
hsuan2481 0:d7739916cfb1 246
hsuan2481 0:d7739916cfb1 247 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
hsuan2481 0:d7739916cfb1 248 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
hsuan2481 0:d7739916cfb1 249 pwm2_duty = PI_out_2 + 0.5f;
hsuan2481 0:d7739916cfb1 250 pwm2.write(PI_out_2 + 0.5f);
hsuan2481 0:d7739916cfb1 251 TIM1->CCER |= 0x40;
hsuan2481 0:d7739916cfb1 252 }