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 //
00040 float R = 0.09; // m unit
00041 int get_pluse_L , get_pluse_R = 0;
00042 float Omega_e_L, Omega_w_L, V_real_L;
00043 float Omega_e_R, Omega_w_R, V_real_R;
00044 float test_time_2;
00045 float test_time;
00046 float V_array[2];
00047 float raw_test[2]; //old duble
00048 float V_raw, Omega_raw; //old duble
00049 float V_cal_L, V_cal_R;
00050 float b = 0.39; // distan wheel_L to wheel_R
00051 float rpm_set = 0.42;
00052 float v_set = 0.25;
00053 float pre_pwm_L, pre_pwm_R; 
00054 
00055 
00056 //float get_direction(float Wh_L,float Wh_R){
00057 //    
00058 //    }
00059 
00060 
00061 
00062 void get_wheel_L() {
00063     //
00064     test_time = timer.read();
00065     get_pluse_L = wheel_L.getPulses();
00066     //wait(1);
00067     get_pluse_R = wheel_R.getPulses();
00068     //
00069     Omega_e_L = (2 * 3.14 * get_pluse_L)/(2000 * 0.05); //0.1 is preiod
00070     //
00071     Omega_e_R = (2 * 3.14 * get_pluse_R)/(2000 * 0.05); //0.1 is preiod
00072     //
00073     Omega_w_L = Omega_e_L / 4; 
00074     //
00075     Omega_w_R = Omega_e_R / 4;
00076     //
00077     V_real_L = Omega_w_L * R;
00078     //
00079     V_real_R = Omega_w_R * R;
00080     //
00081     wheel_L.reset();
00082     //
00083     wheel_R.reset(); //real had
00084    
00085 }
00086 
00087 void Send_V_to_ros(){
00088            
00089         V_array[0] = V_real_L;
00090         V_array[1] = V_real_R;
00091         //V_array[0] = get_pluse_L;
00092         //V_array[1] = get_pluse_R;
00093         Array_msg.data = V_array;
00094         chatter_test.publish( &Array_msg );
00095         //printf ("VL: %.2f VR: %.2f \n",V_array[0],V_array[1]);
00096         nh.spinOnce();
00097     }
00098     
00099 void messageCb(const geometry_msgs::Twist& toggle_msg){
00100         // convest to vL vR
00101         V_raw = toggle_msg.linear.x;
00102         Omega_raw = toggle_msg.angular.z;
00103         V_cal_L = ((2*V_raw)-(Omega_raw*b))/2;
00104         V_cal_R = ((2*V_raw)+(Omega_raw*b))/2;
00105         //
00106         pre_pwm_L = (V_cal_L * rpm_set)/v_set;    // rpm motor L
00107         pre_pwm_R = (V_cal_R * rpm_set)/v_set;    // rpm motor R
00108         
00109         //direction 
00110         if((pre_pwm_L < 0)&&(pre_pwm_R > 0)){
00111             L_NA = 1;
00112             L_NB = 0;
00113             //
00114             R_NA = 0;
00115             R_NB = 1;
00116          
00117             }
00118         else if((pre_pwm_L > 0)&&(pre_pwm_R < 0)){
00119             L_NA = 0;
00120             L_NB = 1;
00121             //
00122             R_NA = 1;
00123             R_NB = 0;
00124 
00125             }
00126         else if((pre_pwm_L)&&(pre_pwm_R) > 0){
00127             L_NA = 0;
00128             L_NB = 1;
00129             //
00130             R_NA = 0;
00131             R_NB = 1;
00132            
00133             }
00134          else if((pre_pwm_L)&&(pre_pwm_R) < 0){
00135             L_NA = 1;
00136             L_NB = 0;
00137             //
00138             R_NA = 1;
00139             R_NB = 0;
00140             }
00141       
00142         mypwm_L = abs(pre_pwm_L);
00143         mypwm_R = abs(pre_pwm_R);
00144         //****recheck******
00145       // raw_test[0] = pre_pwm_L;
00146 //        raw_test[1] = pre_pwm_R;
00147 //        x_line_msg.data = raw_test;
00148 //        mbed_send.publish(&x_line_msg);
00149 //        nh.spinOnce();
00150 
00151     }
00152 
00153 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
00154     
00155 int main() {
00156     
00157     //................
00158     nh.initNode();
00159     nh.advertise(chatter_test);
00160     nh.advertise(mbed_send);
00161     nh.subscribe(sub);
00162     //Array_msg.layout.dim_length = 1;
00163     Array_msg.data_length = 2;
00164     //
00165     x_line_msg.data_length = 2;
00166     //................
00167     Time_L.attach(&get_wheel_L, 0.05); // the address of the function to be attached (get_wheel_L) and the interval (1 seconds)
00168     Send_data.attach(&Send_V_to_ros,0.05);
00169     
00170     mypwm_L.period(0.0005);  
00171     mypwm_R.period(0.0005);
00172    // mypwm_L = 0;
00173 //    mypwm_R = 0;
00174     
00175     while (1) {
00176         nh.spinOnce();
00177         wait_ms(1);
00178     }
00179     
00180 }