main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include <ros.h>
00004 #include <std_msgs/String.h>
00005 #include <std_msgs/Float32MultiArray.h>
00006 #include <std_msgs/Float64MultiArray.h>
00007 #include <std_msgs/Float64.h>
00008 #include <geometry_msgs/Twist.h>
00009 //
00010 ros::NodeHandle  nh;
00011 std_msgs::String str_msg;
00012 std_msgs::Float32MultiArray Array_msg;
00013 ros::Publisher chatter_test("chatter", &Array_msg);
00014 //
00015 std_msgs::Float32MultiArray x_line_msg;
00016 ros::Publisher mbed_send("mbed_to_ros", &x_line_msg);
00017 //
00018 
00019 //
00020 Timer timer;
00021 Timer timer_2;
00022 //
00023 Ticker Time_L;
00024 Ticker Time_R;
00025 Ticker Send_data;
00026 //
00027 PwmOut mypwm_L(PA_1); 
00028 PwmOut mypwm_R(PC_7);
00029 //
00030 QEI wheel_R(PB_5, PB_4 ,NC,500,QEI::X4_ENCODING);   // left wheel
00031 QEI wheel_L(PA_10, PB_3 ,NC,500,QEI::X4_ENCODING); //right wheeel
00032 //
00033 DigitalOut L_NA(PB_0); //old 0 FW
00034 DigitalOut L_NB(PC_1); //old 1
00035 //
00036 DigitalOut R_NA(PA_7); //old 0
00037 DigitalOut R_NB(PB_6); //old 1
00038 
00039 DigitalIn IR(PA_8); //D7
00040 DigitalOut led(LED1);
00041 //
00042 float R = 0.09; // m unit
00043 int get_pluse_L , get_pluse_R = 0;
00044 float Omega_e_L, Omega_w_L, V_real_L;
00045 float Omega_e_R, Omega_w_R, V_real_R;
00046 float test_time_2;
00047 float test_time;
00048 float V_array[3];
00049 float raw_test[2]; //old duble
00050 float V_raw, Omega_raw; //old duble
00051 float V_cal_L, V_cal_R;
00052 float b = 0.39; // distan wheel_L to wheel_R
00053 float rpm_set = 0.42;
00054 float v_set = 0.25;
00055 float pre_pwm_L, pre_pwm_R; 
00056 
00057 
00058 //float get_direction(float Wh_L,float Wh_R){
00059 //    
00060 //    }
00061 
00062 
00063 
00064 void get_wheel_L() {
00065     //
00066     test_time = timer.read();
00067     get_pluse_L = wheel_L.getPulses();
00068     //wait(1);
00069     get_pluse_R = wheel_R.getPulses();
00070     //
00071     Omega_e_L = (2 * 3.14 * get_pluse_L)/(2000 * 0.05); //0.1 is preiod
00072     //
00073     Omega_e_R = (2 * 3.14 * get_pluse_R)/(2000 * 0.05); //0.1 is preiod
00074     //
00075     Omega_w_L = Omega_e_L / 4; 
00076     //
00077     Omega_w_R = Omega_e_R / 4;
00078     //
00079     V_real_L = Omega_w_L * R;
00080     //
00081     V_real_R = Omega_w_R * R;
00082     //
00083     wheel_L.reset();
00084     //
00085     wheel_R.reset(); //real had
00086    
00087 }
00088 
00089 void Send_V_to_ros(){
00090            
00091         V_array[0] = V_real_L;
00092         V_array[1] = V_real_R;
00093         V_array[2] = 0;
00094         
00095         if (IR == 1){
00096             V_array[2] = 1;
00097             led = 0;
00098             }
00099         else if (IR == 0){
00100             V_array[2] = 0;
00101             led = 1;
00102             }
00103         //V_array[0] = get_pluse_L;
00104         //V_array[1] = get_pluse_R;
00105         Array_msg.data = V_array;
00106         chatter_test.publish( &Array_msg );
00107         //printf ("VL: %.2f VR: %.2f \n",V_array[0],V_array[1]);
00108         nh.spinOnce();
00109     }
00110     
00111 void messageCb(const geometry_msgs::Twist& toggle_msg){
00112         // convest to vL vR
00113         V_raw = toggle_msg.linear.x;
00114         Omega_raw = toggle_msg.angular.z;
00115         V_cal_L = ((2*V_raw)-(Omega_raw*b))/2;
00116         V_cal_R = ((2*V_raw)+(Omega_raw*b))/2;
00117         //
00118         pre_pwm_L = (V_cal_L * rpm_set)/v_set;    // rpm motor L
00119         pre_pwm_R = (V_cal_R * rpm_set)/v_set;    // rpm motor R
00120         
00121         //direction 
00122         if((pre_pwm_L < 0)&&(pre_pwm_R > 0)){
00123             L_NA = 1;
00124             L_NB = 0;
00125             //
00126             R_NA = 0;
00127             R_NB = 1;
00128          
00129             }
00130         else if((pre_pwm_L > 0)&&(pre_pwm_R < 0)){
00131             L_NA = 0;
00132             L_NB = 1;
00133             //
00134             R_NA = 1;
00135             R_NB = 0;
00136 
00137             }
00138         else if((pre_pwm_L)&&(pre_pwm_R) > 0){
00139             L_NA = 0;
00140             L_NB = 1;
00141             //
00142             R_NA = 0;
00143             R_NB = 1;
00144            
00145             }
00146          else if((pre_pwm_L)&&(pre_pwm_R) < 0){
00147             L_NA = 1;
00148             L_NB = 0;
00149             //
00150             R_NA = 1;
00151             R_NB = 0;
00152             }
00153       
00154         mypwm_L = abs(pre_pwm_L);
00155         mypwm_R = abs(pre_pwm_R);
00156         //****recheck******
00157       // raw_test[0] = pre_pwm_L;
00158 //        raw_test[1] = pre_pwm_R;
00159 //        x_line_msg.data = raw_test;
00160 //        mbed_send.publish(&x_line_msg);
00161 //        nh.spinOnce();
00162 
00163     }
00164 
00165 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
00166     
00167 int main() {
00168     
00169     //................
00170     nh.initNode();
00171     nh.advertise(chatter_test);
00172     nh.advertise(mbed_send);
00173     nh.subscribe(sub);
00174     //Array_msg.layout.dim_length = 1;
00175     Array_msg.data_length = 2;
00176     //
00177     x_line_msg.data_length = 2;
00178     //................
00179     Time_L.attach(&get_wheel_L, 0.05); // the address of the function to be attached (get_wheel_L) and the interval (1 seconds)
00180     Send_data.attach(&Send_V_to_ros,0.05);
00181     
00182     mypwm_L.period(0.0005);  
00183     mypwm_R.period(0.0005);
00184    // mypwm_L = 0;
00185 //    mypwm_R = 0;
00186     led = 1;
00187     while (1) {
00188         nh.spinOnce();
00189         wait_ms(1);
00190     }
00191     
00192 }