first commit
Dependencies: mbed hallsensor_hardware_decoder ros_lib_kinetic
main.cpp
- Committer:
- shadow103012033
- Date:
- 2019-03-27
- Revision:
- 0:e27261bd5773
File content as of revision 0:e27261bd5773:
#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; }