first commit
Dependencies: mbed hallsensor_hardware_decoder ros_lib_kinetic
main.cpp
00001 #include "mbed.h" 00002 #include <ros.h> 00003 #include "hallsensor_software_decoder.h" 00004 #include <geometry_msgs/Twist.h> 00005 00006 #define Ts 0.01f 00007 00008 Ticker timer1; 00009 00010 PwmOut pwm1(D7); 00011 PwmOut pwm1n(D11); 00012 PwmOut pwm2(D8); 00013 PwmOut pwm2n(A3); 00014 00015 void init_TIMER(); 00016 void init_PWM(); 00017 void timer1_ITR(); 00018 00019 00020 float rotation_speed_1 = 0.0; 00021 float rotation_speed_ref_1 = 0; 00022 float pwm1_duty = 0.5; 00023 double PI_out_1 = 0.0; 00024 float err_1 = 0.0; 00025 float ierr_1 = 0.0; 00026 float rotation_speed_2 = 0.0; 00027 float rotation_speed_ref_2 = -0; 00028 float pwm2_duty = 0.5; 00029 float PI_out_2 = 0.0; 00030 float err_2 = 0.0; 00031 float ierr_2 = 0.0; 00032 00033 float r = 0.03; // wheel radius (m) 00034 float L = 0.27; // car width (m) 00035 00036 float V_ref = 0; // (m/s) max: 0.25 00037 float V = 0; 00038 float W_ref = 0; // (rad/s) 00039 float W = 0; 00040 00041 00042 float Kp = 0.008; 00043 float Ki = 0.02; 00044 00045 //Serial pc(USBTX, USBRX); 00046 00047 //rosserial 00048 ros::NodeHandle nh; 00049 00050 geometry_msgs::Twist vel_msg; 00051 ros::Publisher p("feedback_Vel", &vel_msg); 00052 00053 void messageCallback(const geometry_msgs::Twist &msg_receive) 00054 { 00055 V_ref = 0.25*msg_receive.linear.x; 00056 W_ref = msg_receive.angular.z; 00057 00058 } 00059 00060 ros::Subscriber<geometry_msgs::Twist> s("cmd_vel",messageCallback); 00061 00062 int main() 00063 { 00064 //pc.baud(9600); 00065 init_TIMER(); 00066 init_PWM(); 00067 init_CN(); 00068 00069 nh.initNode(); 00070 nh.subscribe(s); 00071 nh.advertise(p); 00072 00073 while(1) { 00074 V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f; 00075 W = (-1.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f; 00076 /* pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2); 00077 pc.printf("V = %f, W = %f\r\n", V,W);*/ 00078 00079 vel_msg.linear.x = V; 00080 vel_msg.angular.z = W; 00081 00082 p.publish(&vel_msg); 00083 00084 nh.spinOnce(); 00085 00086 wait_ms(100); 00087 } 00088 } 00089 00090 00091 00092 void init_TIMER() 00093 { 00094 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds) 00095 } 00096 00097 00098 void init_PWM() 00099 { 00100 pwm1.period_us(50); 00101 pwm1.write(0.5); 00102 TIM1->CCER |= 0x4; 00103 00104 pwm2.period_us(50); 00105 pwm2.write(0.5); 00106 TIM1->CCER |= 0x40; 00107 } 00108 00109 00110 void timer1_ITR() 00111 { 00112 00113 rotation_speed_ref_1 = (V_ref / r - L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f; 00114 rotation_speed_ref_2 = (V_ref / r + L * W_ref / 2.0f / r) / 2.0f / 3.14f * 60.0f; 00115 00116 00117 // motor1 00118 00119 rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm 00120 speed_count_1 = 0; 00121 00122 ///PI controller for motor1/// 00123 00124 err_1 = rotation_speed_ref_1 - rotation_speed_1; 00125 ierr_1 = ierr_1 + Ts * err_1; 00126 if(ierr_1 > 50.0f) 00127 { 00128 ierr_1 = 50.0; 00129 } 00130 else if(ierr_1 < -50.0f) 00131 { 00132 ierr_1 = -50.0; 00133 } 00134 PI_out_1 = (Kp * err_1 + Ki * ierr_1); 00135 00136 ////////////////////////////// 00137 00138 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; 00139 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; 00140 pwm1_duty = PI_out_1 + 0.5f; 00141 pwm1.write(pwm1_duty); 00142 TIM1->CCER |= 0x4; 00143 00144 00145 00146 00147 //motor2 00148 00149 00150 rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm 00151 speed_count_2 = 0; 00152 00153 ///PI controller for motor2/// 00154 err_2 = rotation_speed_ref_2 - rotation_speed_2; 00155 ierr_2 = ierr_2 + Ts * err_2; 00156 if(ierr_2 > 50.0f) 00157 { 00158 ierr_2 = 50.0; 00159 } 00160 else if(ierr_2 < -50.0f) 00161 { 00162 ierr_2 = -50.0; 00163 } 00164 PI_out_2 = -(Kp * err_2 + Ki * ierr_2) ; 00165 00166 00167 ////////////////////////////// 00168 00169 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; 00170 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; 00171 pwm2_duty = PI_out_2 + 0.5f; 00172 pwm2.write(pwm2_duty); 00173 TIM1->CCER |= 0x40; 00174 00175 00176 }
Generated on Thu Jul 14 2022 03:02:23 by
1.7.2