0edi
Dependencies: mbed ros_lib_indigo
Diff: main.cpp
- Revision:
- 0:d264484486c3
- Child:
- 1:9238d61200e0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 29 02:46:45 2017 +0000 @@ -0,0 +1,313 @@ +#include "mbed.h" +#include <ros.h> +#include <geometry_msgs/Vector3.h> +#include <geometry_msgs/Twist.h> + +#define Ts 0.01f //period of timer1 (s) + +Serial pc(USBTX, USBRX); + +DigitalOut led1(PC_1); + +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_TIMER(void); +void timer1_interrupt(void); +// 函式宣告 +void init_IO(); +void init_CN(); +void CN_ITR(); +void init_PWM(); + +// servo motor +float servo_duty = 0.069; // 0.069 +(0.088/180)*angle, -90<angle<90 +// 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 +int angle = 0; + +// 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; + +// DC motor rotation speed control +int speed_count_1 = 0; +float rotation_speed_1 = 0.0; +float rotation_speed_ref_1 = 0.0;//150rpm +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.0;//-80rpm +float pwm2_duty = 0.5; +float PI_out_2 = 0.0; +float err_2 = 0.0; +float ierr_2 = 0.0; + +//set parameters +float kp = 0.015; +float ki = 0.11; +float pi = 3.14159; + +ros::NodeHandle nh; + +geometry_msgs::Twist vel_msg; +ros::Publisher p("feedback_wheel_angularVel", &vel_msg); + +void messageCb(const geometry_msgs::Vector3 &msg){ + //receive velocity command from PC to motor + + geometry_msgs::Vector3 vector3 = msg; + + rotation_speed_ref_2 = (vector3.x)/1000*2*pi/60; + rotation_speed_ref_1 = (vector3.y)/1000*2*pi/60; + +} +ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel", messageCb); + +int main() { + led1 = 0; + init_TIMER(); + init_PWM(); + init_CN(); + + nh.initNode(); + nh.subscribe(s); + nh.advertise(p); + + while(1) { + vel_msg.linear.x = rotation_speed_ref_2; //command1 + vel_msg.linear.y = rotation_speed_2; //respond1 + vel_msg.linear.z = 0; + + vel_msg.angular.x = rotation_speed_ref_1; //command1 + vel_msg.angular.y = rotation_speed_1; //respond1 + vel_msg.angular.z = 0; + } +} + +void timer1_interrupt(){ + //do motor control here + + //pc.printf("hello timer1_ITR\n"); + // servo motor + ///code for sevo motor/// + ///////////////////////// + ///////////////////////// + //rotating angle for every call + angle = 15; + + //servo_duty output for every call + servo_duty = servo_duty + 0.088/180*angle/100; + ///////////////////////// + ///////////////////////// + + //protection code for servo + 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/// + ////////////////////////////// + ////////////////////////////// + + //PI control + err_1 = rotation_speed_ref_1 - rotation_speed_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; + //pc.printf("pwm1_duty = %d \n\r", pwm1_duty); + pwm1.write(pwm1_duty); + 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/// + ////////////////////////////// + ////////////////////////////// + + //PI control + err_2 = rotation_speed_ref_2 - rotation_speed_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; + //pc.printf("pwm2_duty = %d \n\r", pwm2_duty); + pwm2.write(pwm2_duty); + TIM1->CCER |= 0x40; +} + +void init_TIMER(){ + timer1.attach_us(&timer1_interrupt, 10000); +} + +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() +{ + //pc.printf("hello init_CN \n"); + 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(); + //led1 != led1; + + ///code for state determination/// + ////////////////////////////////// + ////////////////////////////////// + //determine the state + if((HallA_1_state == 0)&&(HallB_1_state == 0)) + { + state_1 = 1; + } + else if((HallA_1_state == 0)&&(HallB_1_state == 1)) + { + state_1 = 2; + } + else if((HallA_1_state == 1)&&(HallB_1_state == 1)) + { + state_1 = 3; + } + else if((HallA_1_state == 1)&&(HallB_1_state ==0)) + { + state_1 = 4; + } + + //forward or backward + int direction_1 = 0; + direction_1 = state_1 - state_1_old; + if((direction_1 == -1) || (direction_1 == 3)) + { + //forward + speed_count_1 = speed_count_1 + 1; + } + else if((direction_1 == 1) || (direction_1 == -3)) + { + //backward + speed_count_1 = speed_count_1 - 1; + } + else + { + //prevent initializing error + state_1_old = state_1; + } + + //change old state + 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/// + ////////////////////////////////// + ///////////////////////////////// + //determine the state + if((HallA_2_state == 0)&&(HallB_2_state == 0)) + { + state_2 = 1; + } + else if((HallA_2_state == 0)&&(HallB_2_state == 1)) + { + state_2 = 2; + } + else if((HallA_2_state == 1)&&(HallB_2_state == 1)) + { + state_2 = 3; + } + else if((HallA_2_state == 1)&&(HallB_2_state ==0)) + { + state_2 = 4; + } + + //forward or backward + int direction_2 = 0; + direction_2 = state_2 - state_2_old; + if((direction_2 == 1) || (direction_2 == -3)) + { + //forward + speed_count_2 = speed_count_2 + 1; + } + else if((direction_2 == -1) || (direction_2 == 3)) + { + //backward + speed_count_2 = speed_count_2 - 1; + } + else + { + //prevent initializing error + state_2_old = state_2; + } + + //change old state + state_2_old = state_2; + ////////////////////////////////// + ////////////////////////////////// + //forward : speed_count_2 + 1 + //backward : speed_count_2 - 1 +}