ROBOTIC CHAMPION TEAM / Mbed 2 deprecated Nucleo_DC_SERVO_PREMIUM

1

Dependencies:   mbed

main.cpp@0:0ad00cf2ed98, 2017-03-24 (annotated)

Committer:
hsuan2481
Date:
Fri Mar 24 06:04:06 2017 +0000
Revision:
0:0ad00cf2ed98
1

Who changed what in which revision?

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