Diff: main.cpp

Revision:

0:18fcf04b703b

Child:

1:5c48d64f17cb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 29 05:12:42 2019 +0000
@@ -0,0 +1,180 @@
+#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);
+    }
+    
+}
\ No newline at end of file