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?
User | Revision | Line number | New 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 | } |