File content as of revision 0:18fcf04b703b:

#include "mbed.h"
#include "QEI.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
//
ros::NodeHandle  nh;
std_msgs::String str_msg;
std_msgs::Float32MultiArray Array_msg;
ros::Publisher chatter_test("chatter", &Array_msg);
//
std_msgs::Float32MultiArray x_line_msg;
ros::Publisher mbed_send("mbed_to_ros", &x_line_msg);
//

//
Timer timer;
Timer timer_2;
//
Ticker Time_L;
Ticker Time_R;
Ticker Send_data;
//
PwmOut mypwm_L(PA_1); 
PwmOut mypwm_R(PC_7);
//
QEI wheel_R(PB_5, PB_4 ,NC,500,QEI::X4_ENCODING);   // left wheel
QEI wheel_L(PA_10, PB_3 ,NC,500,QEI::X4_ENCODING); //right wheeel
//
DigitalOut L_NA(PB_0); //old 0 FW
DigitalOut L_NB(PC_1); //old 1
//
DigitalOut R_NA(PA_7); //old 0
DigitalOut R_NB(PB_6); //old 1

//
float R = 0.09; // m unit
int get_pluse_L , get_pluse_R = 0;
float Omega_e_L, Omega_w_L, V_real_L;
float Omega_e_R, Omega_w_R, V_real_R;
float test_time_2;
float test_time;
float V_array[2];
float raw_test[2]; //old duble
float V_raw, Omega_raw; //old duble
float V_cal_L, V_cal_R;
float b = 0.39; // distan wheel_L to wheel_R
float rpm_set = 0.42;
float v_set = 0.25;
float pre_pwm_L, pre_pwm_R; 


//float get_direction(float Wh_L,float Wh_R){
//    
//    }



void get_wheel_L() {
    //
    test_time = timer.read();
    get_pluse_L = wheel_L.getPulses();
    //wait(1);
    get_pluse_R = wheel_R.getPulses();
    //
    Omega_e_L = (2 * 3.14 * get_pluse_L)/(2000 * 0.05); //0.1 is preiod
    //
    Omega_e_R = (2 * 3.14 * get_pluse_R)/(2000 * 0.05); //0.1 is preiod
    //
    Omega_w_L = Omega_e_L / 4; 
    //
    Omega_w_R = Omega_e_R / 4;
    //
    V_real_L = Omega_w_L * R;
    //
    V_real_R = Omega_w_R * R;
    //
    wheel_L.reset();
    //
    wheel_R.reset(); //real had
   
}

void Send_V_to_ros(){
           
        V_array[0] = V_real_L;
        V_array[1] = V_real_R;
        //V_array[0] = get_pluse_L;
        //V_array[1] = get_pluse_R;
        Array_msg.data = V_array;
        chatter_test.publish( &Array_msg );
        //printf ("VL: %.2f VR: %.2f \n",V_array[0],V_array[1]);
        nh.spinOnce();
    }
    
void messageCb(const geometry_msgs::Twist& toggle_msg){
        // convest to vL vR
        V_raw = toggle_msg.linear.x;
        Omega_raw = toggle_msg.angular.z;
        V_cal_L = ((2*V_raw)-(Omega_raw*b))/2;
        V_cal_R = ((2*V_raw)+(Omega_raw*b))/2;
        //
        pre_pwm_L = (V_cal_L * rpm_set)/v_set;    // rpm motor L
        pre_pwm_R = (V_cal_R * rpm_set)/v_set;    // rpm motor R
        
        //direction 
        if((pre_pwm_L < 0)&&(pre_pwm_R > 0)){
            L_NA = 1;
            L_NB = 0;
            //
            R_NA = 0;
            R_NB = 1;
         
            }
        else if((pre_pwm_L > 0)&&(pre_pwm_R < 0)){
            L_NA = 0;
            L_NB = 1;
            //
            R_NA = 1;
            R_NB = 0;

            }
        else if((pre_pwm_L)&&(pre_pwm_R) > 0){
            L_NA = 0;
            L_NB = 1;
            //
            R_NA = 0;
            R_NB = 1;
           
            }
         else if((pre_pwm_L)&&(pre_pwm_R) < 0){
            L_NA = 1;
            L_NB = 0;
            //
            R_NA = 1;
            R_NB = 0;
            }
      
        mypwm_L = abs(pre_pwm_L);
        mypwm_R = abs(pre_pwm_R);
        //****recheck******
      // raw_test[0] = pre_pwm_L;
//        raw_test[1] = pre_pwm_R;
//        x_line_msg.data = raw_test;
//        mbed_send.publish(&x_line_msg);
//        nh.spinOnce();

    }

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
    
int main() {
    
    //................
    nh.initNode();
    nh.advertise(chatter_test);
    nh.advertise(mbed_send);
    nh.subscribe(sub);
    //Array_msg.layout.dim_length = 1;
    Array_msg.data_length = 2;
    //
    x_line_msg.data_length = 2;
    //................
    Time_L.attach(&get_wheel_L, 0.05); // the address of the function to be attached (get_wheel_L) and the interval (1 seconds)
    Send_data.attach(&Send_V_to_ros,0.05);
    
    mypwm_L.period(0.0005);  
    mypwm_R.period(0.0005);
   // mypwm_L = 0;
//    mypwm_R = 0;
    
    while (1) {
        nh.spinOnce();
        wait_ms(1);
    }
    
}