lab5
Dependencies: mbed ros_lib_indigo
Revision 0:0846fa975a83, committed 2017-03-28
- Comitter:
- tea5062001
- Date:
- Tue Mar 28 13:14:57 2017 +0000
- Commit message:
- lab5
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Mar 28 13:14:57 2017 +0000 @@ -0,0 +1,318 @@ +#include "mbed.h" +#include <ros.h> +#include <geometry_msgs/Twist.h> +#include <geometry_msgs/Vector3.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 = 0; +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 = 0; +float pwm2_duty = 0.5; +float PI_out_2 = 0.0; +float err_2 = 0.0; +float ierr_2 = 0.0; + +//rosserial +ros::NodeHandle nh; + +geometry_msgs::Twist vel_msg; +ros::Publisher p("feedback_wheel_angularVel", &vel_msg); + +void messageCallback(const geometry_msgs::Vector3 &msg_receive) +{ + rotation_speed_ref_1 = -msg_receive.x; + rotation_speed_ref_2 = msg_receive.y; +} + +ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel",messageCallback); + +int main() +{ + init_TIMER(); + init_PWM(); + init_CN(); + + nh.initNode(); + nh.subscribe(s); + nh.advertise(p); + + while(1) + { + vel_msg.linear.x = rotation_speed_ref_1; + vel_msg.linear.y = rotation_speed_1; + vel_msg.linear.z = 0; + + vel_msg.angular.x = rotation_speed_ref_2; + vel_msg.angular.y = rotation_speed_2; + vel_msg.angular.z = 0; + + p.publish(&vel_msg); + + nh.spinOnce(); + wait_ms(20); + } +} + +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/// + + state_1 = 10*HallA_1_state + HallB_1_state; //state = AB (ex:A=1,B=0, state_1 = 10) + + if(state_1_old != state_1) + { + if((state_1_old/10) == (state_1_old%10)) + { + if((state_1%10) != (state_1_old%10)) + { + speed_count_1++; + } + else if((state_1/10) != (state_1_old/10)) + { + speed_count_1--; + } + } + else if((state_1_old/10) != (state_1_old%10)) + { + if((state_1%10) != (state_1_old%10)) + { + speed_count_1--; + } + else if((state_1/10) != (state_1_old/10)) + { + speed_count_1++; + } + } + + state_1_old = state_1; + } + + + ////////////////////////////////// + + //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/// + + state_2 = 10*HallA_2_state + HallB_2_state; //state = AB (ex:A=1,B=0, state_1 = 10) + + if(state_2_old != state_2) + { + if((state_2_old/10) == (state_2_old%10)) + { + if((state_2%10) != (state_2_old%10)) + { + speed_count_2++; + } + else if((state_2/10) != (state_2_old/10)) + { + speed_count_2--; + } + } + else if((state_2_old/10) != (state_2_old%10)) + { + if((state_2%10) != (state_2_old%10)) + { + speed_count_2--; + } + else if((state_2/10) != (state_2_old/10)) + { + speed_count_2++; + } + } + + state_2_old = state_2; + } + + + + ////////////////////////////////// + + //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 Tue Mar 28 13:14:57 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/093f2bd7b9eb \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_indigo.lib Tue Mar 28 13:14:57 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/garyservin/code/ros_lib_indigo/#fd24f7ca9688