1
Dependencies: mbed
Revision 0:d7739916cfb1, committed 2017-03-23
- Comitter:
- hsuan2481
- Date:
- Thu Mar 23 06:33:10 2017 +0000
- Commit message:
- ROBOTIC CHAMPION TEAM
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 23 06:33:10 2017 +0000 @@ -0,0 +1,252 @@ +#include "mbed.h" + +//The number will be compiled as type "double" in default +//Add a "f" after the number can make it compiled as type "float" +#define Ts 0.01f //period of timer1 (s) +#define Kp 0.006f +#define Ki 0.02f + +Ticker timer1; +// servo motor +PwmOut servo_cmd(A0); +// DC motor +PwmOut pwm1(D7); +PwmOut pwm1n(D11); +PwmOut pwm2(D8); +PwmOut pwm2n(A3); + +// Motor1 sensor +InterruptIn HallA(A1); +InterruptIn HallB(A2); +// Motor2 sensor +InterruptIn HallA_2(D13); +InterruptIn HallB_2(D12); + +// 函式宣告 +void init_IO(); +void init_TIMER(); +void timer1_ITR(); +void init_CN(); +void CN_ITR(); +void init_PWM(); + +// servo motor +float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90 +// 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 +int angle = 0; +int counter; + + +// Hall sensor +int HallA_1_state = 0; +int HallB_1_state = 0; +int state_1 = 0; +int state_1_old = 0; +int HallA_2_state = 0; +int HallB_2_state = 0; +int state_2 = 0; +int state_2_old = 0; + +int c = 0; +int d = 0; + +// DC motor rotation speed control +int speed_count_1 = 0; +float rotation_speed_1 = 0.0; +float rotation_speed_ref_1 = 150; +float pwm1_duty = 0.5; +float PI_out_1 = 0.0; +float err_1 = 0.0; +float ierr_1 = 0.0; +int speed_count_2 = 0; +float rotation_speed_2 = 0.0; +float rotation_speed_ref_2 =80; +float pwm2_duty = 0.5; +float PI_out_2 = 0.0; +float err_2 = 0.0; +float ierr_2 = 0.0; + +int main() +{ + init_TIMER(); + init_PWM(); + init_CN(); + + while(1) { + } +} + +void init_TIMER() +{ + timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds) +} + +void init_PWM() +{ + servo_cmd.period_ms(20); + servo_cmd.write(servo_duty); + + pwm1.period_us(50); + pwm1.write(0.5); + TIM1->CCER |= 0x4; + + pwm2.period_us(50); + pwm2.write(0.5); + TIM1->CCER |= 0x40; +} +void init_CN() +{ + HallA.rise(&CN_ITR); + HallA.fall(&CN_ITR); + HallB.rise(&CN_ITR); + HallB.fall(&CN_ITR); + + HallA_2.rise(&CN_ITR); + HallA_2.fall(&CN_ITR); + HallB_2.rise(&CN_ITR); + HallB_2.fall(&CN_ITR); +} + +void CN_ITR() +{ + // motor1 + HallA_1_state = HallA.read(); + HallB_1_state = HallB.read(); + + ///code for state determination/// + + + if(HallA_1_state == state_1_old && c == 1) + { + speed_count_1 = speed_count_1 + 1; + } + + if(HallB_1_state == state_1_old && c == 1) + { + speed_count_1 = speed_count_1 - 1; + } + + c = 1; + state_1_old = HallA_1_state; + state_1_old = HallB_1_state; + + + ////////////////////////////////// + + //forward : speed_count_1 + 1 + //backward : speed_count_1 - 1 + + + // motor2 + HallA_2_state = HallA_2.read(); + HallB_2_state = HallB_2.read(); + + ///code for state determination/// + + + if(HallA_2_state == state_2_old && d == 1) + { + speed_count_2 = speed_count_2 + 1; + } + + if(HallB_2_state == state_2_old && d == 1) + { + speed_count_2 = speed_count_2 - 1; + } + + d = 1; + state_2_old = HallA_2_state; + state_2_old = HallB_2_state; + + ////////////////////////////////// + + //forward : speed_count_2 + 1 + //backward : speed_count_2 - 1 +} + +void timer1_ITR() +{ + // servo motor + ///code for sevo motor/// + + counter = counter + 1; + + if(counter == 100) + { + servo_duty = 0.069; + } + if(counter == 200) + { + servo_duty = 0.0763; + } + if(counter == 300) + { + servo_duty = 0.0837; + } + if(counter == 400) + { + servo_duty = 0.091; + } + if(counter == 500) + { + servo_duty = 0.0983; + } + if(counter == 600) + { + servo_duty = 0.106; + } + + if(counter == 700) + { + servo_duty = 0.113; + } + if(counter > 700) + { + counter=0; + } + + servo_cmd.write(servo_duty); + + + ///////////////////////// + + if(servo_duty >= 0.113f)servo_duty = 0.113; + else if(servo_duty <= 0.025f)servo_duty = 0.025; + servo_cmd.write(servo_duty); + + // motor1 + rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm + speed_count_1 = 0; + + ///PI controller for motor1/// + + err_1 = rotation_speed_ref_1 - rotation_speed_1; + ierr_1 = ierr_1 + err_1*Ts; + PI_out_1 = Kp*err_1 + Ki*ierr_1; + + ////////////////////////////// + + if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; + else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; + pwm1_duty = PI_out_1 + 0.5f; + pwm1.write(PI_out_1 + 0.5f); + TIM1->CCER |= 0x4; + + //motor2 + rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm + speed_count_2 = 0; + + ///PI controller for motor2/// + + err_2 = rotation_speed_ref_2 - rotation_speed_2; + ierr_2 = ierr_2 + err_2*Ts; + PI_out_2 = Kp*err_2 + Ki*ierr_2; + + ////////////////////////////// + + if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; + else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; + pwm2_duty = PI_out_2 + 0.5f; + pwm2.write(PI_out_2 + 0.5f); + TIM1->CCER |= 0x40; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Mar 23 06:33:10 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/093f2bd7b9eb \ No newline at end of file