first commit
Dependencies: mbed hallsensor_hardware_decoder ros_lib_kinetic
Revision 0:e27261bd5773, committed 2019-03-27
- Comitter:
- shadow103012033
- Date:
- Wed Mar 27 07:07:21 2019 +0000
- Commit message:
- TA code;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hallsensor_hardware_decoder.lib Wed Mar 27 07:07:21 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/mobile_robot_tea/code/hallsensor_hardware_decoder/#c685617c45e6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 27 07:07:21 2019 +0000 @@ -0,0 +1,176 @@ +#include "mbed.h" +#include <ros.h> +#include "hallsensor_software_decoder.h" +#include <geometry_msgs/Twist.h> + +#define Ts 0.01f + +Ticker timer1; + +PwmOut pwm1(D7); +PwmOut pwm1n(D11); +PwmOut pwm2(D8); +PwmOut pwm2n(A3); + +void init_TIMER(); +void init_PWM(); +void timer1_ITR(); + + +float rotation_speed_1 = 0.0; +float rotation_speed_ref_1 = 0; +float pwm1_duty = 0.5; +double PI_out_1 = 0.0; +float err_1 = 0.0; +float ierr_1 = 0.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; + +float r = 0.03; // wheel radius (m) +float L = 0.27; // car width (m) + +float V_ref = 0; // (m/s) max: 0.25 +float V = 0; +float W_ref = 0; // (rad/s) +float W = 0; + + +float Kp = 0.008; +float Ki = 0.02; + +//Serial pc(USBTX, USBRX); + +//rosserial +ros::NodeHandle nh; + +geometry_msgs::Twist vel_msg; +ros::Publisher p("feedback_Vel", &vel_msg); + +void messageCallback(const geometry_msgs::Twist &msg_receive) +{ + V_ref = 0.25*msg_receive.linear.x; + W_ref = msg_receive.angular.z; + +} + +ros::Subscriber<geometry_msgs::Twist> s("cmd_vel",messageCallback); + +int main() +{ + //pc.baud(9600); + init_TIMER(); + init_PWM(); + init_CN(); + + nh.initNode(); + nh.subscribe(s); + nh.advertise(p); + + while(1) { + V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f; + W = (-1.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f; + /* pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2); + pc.printf("V = %f, W = %f\r\n", V,W);*/ + + vel_msg.linear.x = V; + vel_msg.angular.z = W; + + p.publish(&vel_msg); + + nh.spinOnce(); + + wait_ms(100); + } +} + + + +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() +{ + pwm1.period_us(50); + pwm1.write(0.5); + TIM1->CCER |= 0x4; + + pwm2.period_us(50); + pwm2.write(0.5); + TIM1->CCER |= 0x40; +} + + +void timer1_ITR() +{ + + rotation_speed_ref_1 = (V_ref / r - L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f; + rotation_speed_ref_2 = (V_ref / r + L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f; + + + // motor1 + + rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm + speed_count_1 = 0; + + ///PI controller for motor1/// + + err_1 = rotation_speed_ref_1 - rotation_speed_1; + ierr_1 = ierr_1 + Ts * err_1; + if(ierr_1 > 50.0f) + { + ierr_1 = 50.0; + } + else if(ierr_1 < -50.0f) + { + ierr_1 = -50.0; + } + 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(pwm1_duty); + TIM1->CCER |= 0x4; + + + + + //motor2 + + + rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm + speed_count_2 = 0; + + ///PI controller for motor2/// + err_2 = rotation_speed_ref_2 - rotation_speed_2; + ierr_2 = ierr_2 + Ts * err_2; + if(ierr_2 > 50.0f) + { + ierr_2 = 50.0; + } + else if(ierr_2 < -50.0f) + { + ierr_2 = -50.0; + } + 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(pwm2_duty); + TIM1->CCER |= 0x40; + + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Mar 27 07:07:21 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Wed Mar 27 07:07:21 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f