thanachot supawasut
tao_velocity_wheel_gup
Dependencies: mbed QEI ros_lib_kinetic
Revision 0:18fcf04b703b, committed 2019-04-29
- Comitter:
- 58340500022
- Date:
- Mon Apr 29 05:12:42 2019 +0000
- Commit message:
- kk
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Mon Apr 29 05:12:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Apr 29 05:12:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Mon Apr 29 05:12:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f