first commit

Dependencies:   mbed hallsensor_hardware_decoder ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }