Tar zoozoo / Mbed 2 deprecated Velocity_PP

IR

Dependencies:   mbed QEI ros_lib_kinetic

main.cpp@0:18fcf04b703b, 2019-04-29 (annotated)

Committer:
58340500022
Date:
Mon Apr 29 05:12:42 2019 +0000
Revision:
0:18fcf04b703b
Child:
1:5c48d64f17cb
kk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
58340500022 0:18fcf04b703b 1 #include "mbed.h"
58340500022 0:18fcf04b703b 2 #include "QEI.h"
58340500022 0:18fcf04b703b 3 #include <ros.h>
58340500022 0:18fcf04b703b 4 #include <std_msgs/String.h>
58340500022 0:18fcf04b703b 5 #include <std_msgs/Float32MultiArray.h>
58340500022 0:18fcf04b703b 6 #include <std_msgs/Float64MultiArray.h>
58340500022 0:18fcf04b703b 7 #include <std_msgs/Float64.h>
58340500022 0:18fcf04b703b 8 #include <geometry_msgs/Twist.h>
58340500022 0:18fcf04b703b 9 //
58340500022 0:18fcf04b703b 10 ros::NodeHandle nh;
58340500022 0:18fcf04b703b 11 std_msgs::String str_msg;
58340500022 0:18fcf04b703b 12 std_msgs::Float32MultiArray Array_msg;
58340500022 0:18fcf04b703b 13 ros::Publisher chatter_test("chatter", &Array_msg);
58340500022 0:18fcf04b703b 14 //
58340500022 0:18fcf04b703b 15 std_msgs::Float32MultiArray x_line_msg;
58340500022 0:18fcf04b703b 16 ros::Publisher mbed_send("mbed_to_ros", &x_line_msg);
58340500022 0:18fcf04b703b 17 //
58340500022 0:18fcf04b703b 18
58340500022 0:18fcf04b703b 19 //
58340500022 0:18fcf04b703b 20 Timer timer;
58340500022 0:18fcf04b703b 21 Timer timer_2;
58340500022 0:18fcf04b703b 22 //
58340500022 0:18fcf04b703b 23 Ticker Time_L;
58340500022 0:18fcf04b703b 24 Ticker Time_R;
58340500022 0:18fcf04b703b 25 Ticker Send_data;
58340500022 0:18fcf04b703b 26 //
58340500022 0:18fcf04b703b 27 PwmOut mypwm_L(PA_1);
58340500022 0:18fcf04b703b 28 PwmOut mypwm_R(PC_7);
58340500022 0:18fcf04b703b 29 //
58340500022 0:18fcf04b703b 30 QEI wheel_R(PB_5, PB_4 ,NC,500,QEI::X4_ENCODING); // left wheel
58340500022 0:18fcf04b703b 31 QEI wheel_L(PA_10, PB_3 ,NC,500,QEI::X4_ENCODING); //right wheeel
58340500022 0:18fcf04b703b 32 //
58340500022 0:18fcf04b703b 33 DigitalOut L_NA(PB_0); //old 0 FW
58340500022 0:18fcf04b703b 34 DigitalOut L_NB(PC_1); //old 1
58340500022 0:18fcf04b703b 35 //
58340500022 0:18fcf04b703b 36 DigitalOut R_NA(PA_7); //old 0
58340500022 0:18fcf04b703b 37 DigitalOut R_NB(PB_6); //old 1
58340500022 0:18fcf04b703b 38
58340500022 0:18fcf04b703b 39 //
58340500022 0:18fcf04b703b 40 float R = 0.09; // m unit
58340500022 0:18fcf04b703b 41 int get_pluse_L , get_pluse_R = 0;
58340500022 0:18fcf04b703b 42 float Omega_e_L, Omega_w_L, V_real_L;
58340500022 0:18fcf04b703b 43 float Omega_e_R, Omega_w_R, V_real_R;
58340500022 0:18fcf04b703b 44 float test_time_2;
58340500022 0:18fcf04b703b 45 float test_time;
58340500022 0:18fcf04b703b 46 float V_array[2];
58340500022 0:18fcf04b703b 47 float raw_test[2]; //old duble
58340500022 0:18fcf04b703b 48 float V_raw, Omega_raw; //old duble
58340500022 0:18fcf04b703b 49 float V_cal_L, V_cal_R;
58340500022 0:18fcf04b703b 50 float b = 0.39; // distan wheel_L to wheel_R
58340500022 0:18fcf04b703b 51 float rpm_set = 0.42;
58340500022 0:18fcf04b703b 52 float v_set = 0.25;
58340500022 0:18fcf04b703b 53 float pre_pwm_L, pre_pwm_R;
58340500022 0:18fcf04b703b 54
58340500022 0:18fcf04b703b 55
58340500022 0:18fcf04b703b 56 //float get_direction(float Wh_L,float Wh_R){
58340500022 0:18fcf04b703b 57 //
58340500022 0:18fcf04b703b 58 // }
58340500022 0:18fcf04b703b 59
58340500022 0:18fcf04b703b 60
58340500022 0:18fcf04b703b 61
58340500022 0:18fcf04b703b 62 void get_wheel_L() {
58340500022 0:18fcf04b703b 63 //
58340500022 0:18fcf04b703b 64 test_time = timer.read();
58340500022 0:18fcf04b703b 65 get_pluse_L = wheel_L.getPulses();
58340500022 0:18fcf04b703b 66 //wait(1);
58340500022 0:18fcf04b703b 67 get_pluse_R = wheel_R.getPulses();
58340500022 0:18fcf04b703b 68 //
58340500022 0:18fcf04b703b 69 Omega_e_L = (2 * 3.14 * get_pluse_L)/(2000 * 0.05); //0.1 is preiod
58340500022 0:18fcf04b703b 70 //
58340500022 0:18fcf04b703b 71 Omega_e_R = (2 * 3.14 * get_pluse_R)/(2000 * 0.05); //0.1 is preiod
58340500022 0:18fcf04b703b 72 //
58340500022 0:18fcf04b703b 73 Omega_w_L = Omega_e_L / 4;
58340500022 0:18fcf04b703b 74 //
58340500022 0:18fcf04b703b 75 Omega_w_R = Omega_e_R / 4;
58340500022 0:18fcf04b703b 76 //
58340500022 0:18fcf04b703b 77 V_real_L = Omega_w_L * R;
58340500022 0:18fcf04b703b 78 //
58340500022 0:18fcf04b703b 79 V_real_R = Omega_w_R * R;
58340500022 0:18fcf04b703b 80 //
58340500022 0:18fcf04b703b 81 wheel_L.reset();
58340500022 0:18fcf04b703b 82 //
58340500022 0:18fcf04b703b 83 wheel_R.reset(); //real had
58340500022 0:18fcf04b703b 84
58340500022 0:18fcf04b703b 85 }
58340500022 0:18fcf04b703b 86
58340500022 0:18fcf04b703b 87 void Send_V_to_ros(){
58340500022 0:18fcf04b703b 88
58340500022 0:18fcf04b703b 89 V_array[0] = V_real_L;
58340500022 0:18fcf04b703b 90 V_array[1] = V_real_R;
58340500022 0:18fcf04b703b 91 //V_array[0] = get_pluse_L;
58340500022 0:18fcf04b703b 92 //V_array[1] = get_pluse_R;
58340500022 0:18fcf04b703b 93 Array_msg.data = V_array;
58340500022 0:18fcf04b703b 94 chatter_test.publish( &Array_msg );
58340500022 0:18fcf04b703b 95 //printf ("VL: %.2f VR: %.2f \n",V_array[0],V_array[1]);
58340500022 0:18fcf04b703b 96 nh.spinOnce();
58340500022 0:18fcf04b703b 97 }
58340500022 0:18fcf04b703b 98
58340500022 0:18fcf04b703b 99 void messageCb(const geometry_msgs::Twist& toggle_msg){
58340500022 0:18fcf04b703b 100 // convest to vL vR
58340500022 0:18fcf04b703b 101 V_raw = toggle_msg.linear.x;
58340500022 0:18fcf04b703b 102 Omega_raw = toggle_msg.angular.z;
58340500022 0:18fcf04b703b 103 V_cal_L = ((2*V_raw)-(Omega_raw*b))/2;
58340500022 0:18fcf04b703b 104 V_cal_R = ((2*V_raw)+(Omega_raw*b))/2;
58340500022 0:18fcf04b703b 105 //
58340500022 0:18fcf04b703b 106 pre_pwm_L = (V_cal_L * rpm_set)/v_set; // rpm motor L
58340500022 0:18fcf04b703b 107 pre_pwm_R = (V_cal_R * rpm_set)/v_set; // rpm motor R
58340500022 0:18fcf04b703b 108
58340500022 0:18fcf04b703b 109 //direction
58340500022 0:18fcf04b703b 110 if((pre_pwm_L < 0)&&(pre_pwm_R > 0)){
58340500022 0:18fcf04b703b 111 L_NA = 1;
58340500022 0:18fcf04b703b 112 L_NB = 0;
58340500022 0:18fcf04b703b 113 //
58340500022 0:18fcf04b703b 114 R_NA = 0;
58340500022 0:18fcf04b703b 115 R_NB = 1;
58340500022 0:18fcf04b703b 116
58340500022 0:18fcf04b703b 117 }
58340500022 0:18fcf04b703b 118 else if((pre_pwm_L > 0)&&(pre_pwm_R < 0)){
58340500022 0:18fcf04b703b 119 L_NA = 0;
58340500022 0:18fcf04b703b 120 L_NB = 1;
58340500022 0:18fcf04b703b 121 //
58340500022 0:18fcf04b703b 122 R_NA = 1;
58340500022 0:18fcf04b703b 123 R_NB = 0;
58340500022 0:18fcf04b703b 124
58340500022 0:18fcf04b703b 125 }
58340500022 0:18fcf04b703b 126 else if((pre_pwm_L)&&(pre_pwm_R) > 0){
58340500022 0:18fcf04b703b 127 L_NA = 0;
58340500022 0:18fcf04b703b 128 L_NB = 1;
58340500022 0:18fcf04b703b 129 //
58340500022 0:18fcf04b703b 130 R_NA = 0;
58340500022 0:18fcf04b703b 131 R_NB = 1;
58340500022 0:18fcf04b703b 132
58340500022 0:18fcf04b703b 133 }
58340500022 0:18fcf04b703b 134 else if((pre_pwm_L)&&(pre_pwm_R) < 0){
58340500022 0:18fcf04b703b 135 L_NA = 1;
58340500022 0:18fcf04b703b 136 L_NB = 0;
58340500022 0:18fcf04b703b 137 //
58340500022 0:18fcf04b703b 138 R_NA = 1;
58340500022 0:18fcf04b703b 139 R_NB = 0;
58340500022 0:18fcf04b703b 140 }
58340500022 0:18fcf04b703b 141
58340500022 0:18fcf04b703b 142 mypwm_L = abs(pre_pwm_L);
58340500022 0:18fcf04b703b 143 mypwm_R = abs(pre_pwm_R);
58340500022 0:18fcf04b703b 144 //****recheck******
58340500022 0:18fcf04b703b 145 // raw_test[0] = pre_pwm_L;
58340500022 0:18fcf04b703b 146 // raw_test[1] = pre_pwm_R;
58340500022 0:18fcf04b703b 147 // x_line_msg.data = raw_test;
58340500022 0:18fcf04b703b 148 // mbed_send.publish(&x_line_msg);
58340500022 0:18fcf04b703b 149 // nh.spinOnce();
58340500022 0:18fcf04b703b 150
58340500022 0:18fcf04b703b 151 }
58340500022 0:18fcf04b703b 152
58340500022 0:18fcf04b703b 153 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
58340500022 0:18fcf04b703b 154
58340500022 0:18fcf04b703b 155 int main() {
58340500022 0:18fcf04b703b 156
58340500022 0:18fcf04b703b 157 //................
58340500022 0:18fcf04b703b 158 nh.initNode();
58340500022 0:18fcf04b703b 159 nh.advertise(chatter_test);
58340500022 0:18fcf04b703b 160 nh.advertise(mbed_send);
58340500022 0:18fcf04b703b 161 nh.subscribe(sub);
58340500022 0:18fcf04b703b 162 //Array_msg.layout.dim_length = 1;
58340500022 0:18fcf04b703b 163 Array_msg.data_length = 2;
58340500022 0:18fcf04b703b 164 //
58340500022 0:18fcf04b703b 165 x_line_msg.data_length = 2;
58340500022 0:18fcf04b703b 166 //................
58340500022 0:18fcf04b703b 167 Time_L.attach(&get_wheel_L, 0.05); // the address of the function to be attached (get_wheel_L) and the interval (1 seconds)
58340500022 0:18fcf04b703b 168 Send_data.attach(&Send_V_to_ros,0.05);
58340500022 0:18fcf04b703b 169
58340500022 0:18fcf04b703b 170 mypwm_L.period(0.0005);
58340500022 0:18fcf04b703b 171 mypwm_R.period(0.0005);
58340500022 0:18fcf04b703b 172 // mypwm_L = 0;
58340500022 0:18fcf04b703b 173 // mypwm_R = 0;
58340500022 0:18fcf04b703b 174
58340500022 0:18fcf04b703b 175 while (1) {
58340500022 0:18fcf04b703b 176 nh.spinOnce();
58340500022 0:18fcf04b703b 177 wait_ms(1);
58340500022 0:18fcf04b703b 178 }
58340500022 0:18fcf04b703b 179
58340500022 0:18fcf04b703b 180 }