Tar zoozoo / Mbed 2 deprecated Velocity_PP

IR

Dependencies:   mbed QEI ros_lib_kinetic

main.cpp@1:5c48d64f17cb, 2019-05-23 (annotated)

Committer:
Tarzoozoo
Date:
Thu May 23 16:47:10 2019 +0000
Revision:
1:5c48d64f17cb
Parent:
0:18fcf04b703b 
with IR;

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
Tarzoozoo 1:5c48d64f17cb 39 DigitalIn IR(PA_8); //D7
Tarzoozoo 1:5c48d64f17cb 40 DigitalOut led(LED1);
58340500022 0:18fcf04b703b 41 //
58340500022 0:18fcf04b703b 42 float R = 0.09; // m unit
58340500022 0:18fcf04b703b 43 int get_pluse_L , get_pluse_R = 0;
58340500022 0:18fcf04b703b 44 float Omega_e_L, Omega_w_L, V_real_L;
58340500022 0:18fcf04b703b 45 float Omega_e_R, Omega_w_R, V_real_R;
58340500022 0:18fcf04b703b 46 float test_time_2;
58340500022 0:18fcf04b703b 47 float test_time;
Tarzoozoo 1:5c48d64f17cb 48 float V_array[3];
58340500022 0:18fcf04b703b 49 float raw_test[2]; //old duble
58340500022 0:18fcf04b703b 50 float V_raw, Omega_raw; //old duble
58340500022 0:18fcf04b703b 51 float V_cal_L, V_cal_R;
58340500022 0:18fcf04b703b 52 float b = 0.39; // distan wheel_L to wheel_R
58340500022 0:18fcf04b703b 53 float rpm_set = 0.42;
58340500022 0:18fcf04b703b 54 float v_set = 0.25;
58340500022 0:18fcf04b703b 55 float pre_pwm_L, pre_pwm_R;
58340500022 0:18fcf04b703b 56
58340500022 0:18fcf04b703b 57
58340500022 0:18fcf04b703b 58 //float get_direction(float Wh_L,float Wh_R){
58340500022 0:18fcf04b703b 59 //
58340500022 0:18fcf04b703b 60 // }
58340500022 0:18fcf04b703b 61
58340500022 0:18fcf04b703b 62
58340500022 0:18fcf04b703b 63
58340500022 0:18fcf04b703b 64 void get_wheel_L() {
58340500022 0:18fcf04b703b 65 //
58340500022 0:18fcf04b703b 66 test_time = timer.read();
58340500022 0:18fcf04b703b 67 get_pluse_L = wheel_L.getPulses();
58340500022 0:18fcf04b703b 68 //wait(1);
58340500022 0:18fcf04b703b 69 get_pluse_R = wheel_R.getPulses();
58340500022 0:18fcf04b703b 70 //
58340500022 0:18fcf04b703b 71 Omega_e_L = (2 * 3.14 * get_pluse_L)/(2000 * 0.05); //0.1 is preiod
58340500022 0:18fcf04b703b 72 //
58340500022 0:18fcf04b703b 73 Omega_e_R = (2 * 3.14 * get_pluse_R)/(2000 * 0.05); //0.1 is preiod
58340500022 0:18fcf04b703b 74 //
58340500022 0:18fcf04b703b 75 Omega_w_L = Omega_e_L / 4;
58340500022 0:18fcf04b703b 76 //
58340500022 0:18fcf04b703b 77 Omega_w_R = Omega_e_R / 4;
58340500022 0:18fcf04b703b 78 //
58340500022 0:18fcf04b703b 79 V_real_L = Omega_w_L * R;
58340500022 0:18fcf04b703b 80 //
58340500022 0:18fcf04b703b 81 V_real_R = Omega_w_R * R;
58340500022 0:18fcf04b703b 82 //
58340500022 0:18fcf04b703b 83 wheel_L.reset();
58340500022 0:18fcf04b703b 84 //
58340500022 0:18fcf04b703b 85 wheel_R.reset(); //real had
58340500022 0:18fcf04b703b 86
58340500022 0:18fcf04b703b 87 }
58340500022 0:18fcf04b703b 88
58340500022 0:18fcf04b703b 89 void Send_V_to_ros(){
58340500022 0:18fcf04b703b 90
58340500022 0:18fcf04b703b 91 V_array[0] = V_real_L;
58340500022 0:18fcf04b703b 92 V_array[1] = V_real_R;
Tarzoozoo 1:5c48d64f17cb 93 V_array[2] = 0;
Tarzoozoo 1:5c48d64f17cb 94
Tarzoozoo 1:5c48d64f17cb 95 if (IR == 1){
Tarzoozoo 1:5c48d64f17cb 96 V_array[2] = 1;
Tarzoozoo 1:5c48d64f17cb 97 led = 0;
Tarzoozoo 1:5c48d64f17cb 98 }
Tarzoozoo 1:5c48d64f17cb 99 else if (IR == 0){
Tarzoozoo 1:5c48d64f17cb 100 V_array[2] = 0;
Tarzoozoo 1:5c48d64f17cb 101 led = 1;
Tarzoozoo 1:5c48d64f17cb 102 }
58340500022 0:18fcf04b703b 103 //V_array[0] = get_pluse_L;
58340500022 0:18fcf04b703b 104 //V_array[1] = get_pluse_R;
58340500022 0:18fcf04b703b 105 Array_msg.data = V_array;
58340500022 0:18fcf04b703b 106 chatter_test.publish( &Array_msg );
58340500022 0:18fcf04b703b 107 //printf ("VL: %.2f VR: %.2f \n",V_array[0],V_array[1]);
58340500022 0:18fcf04b703b 108 nh.spinOnce();
58340500022 0:18fcf04b703b 109 }
58340500022 0:18fcf04b703b 110
58340500022 0:18fcf04b703b 111 void messageCb(const geometry_msgs::Twist& toggle_msg){
58340500022 0:18fcf04b703b 112 // convest to vL vR
58340500022 0:18fcf04b703b 113 V_raw = toggle_msg.linear.x;
58340500022 0:18fcf04b703b 114 Omega_raw = toggle_msg.angular.z;
58340500022 0:18fcf04b703b 115 V_cal_L = ((2*V_raw)-(Omega_raw*b))/2;
58340500022 0:18fcf04b703b 116 V_cal_R = ((2*V_raw)+(Omega_raw*b))/2;
58340500022 0:18fcf04b703b 117 //
58340500022 0:18fcf04b703b 118 pre_pwm_L = (V_cal_L * rpm_set)/v_set; // rpm motor L
58340500022 0:18fcf04b703b 119 pre_pwm_R = (V_cal_R * rpm_set)/v_set; // rpm motor R
58340500022 0:18fcf04b703b 120
58340500022 0:18fcf04b703b 121 //direction
58340500022 0:18fcf04b703b 122 if((pre_pwm_L < 0)&&(pre_pwm_R > 0)){
58340500022 0:18fcf04b703b 123 L_NA = 1;
58340500022 0:18fcf04b703b 124 L_NB = 0;
58340500022 0:18fcf04b703b 125 //
58340500022 0:18fcf04b703b 126 R_NA = 0;
58340500022 0:18fcf04b703b 127 R_NB = 1;
58340500022 0:18fcf04b703b 128
58340500022 0:18fcf04b703b 129 }
58340500022 0:18fcf04b703b 130 else if((pre_pwm_L > 0)&&(pre_pwm_R < 0)){
58340500022 0:18fcf04b703b 131 L_NA = 0;
58340500022 0:18fcf04b703b 132 L_NB = 1;
58340500022 0:18fcf04b703b 133 //
58340500022 0:18fcf04b703b 134 R_NA = 1;
58340500022 0:18fcf04b703b 135 R_NB = 0;
58340500022 0:18fcf04b703b 136
58340500022 0:18fcf04b703b 137 }
58340500022 0:18fcf04b703b 138 else if((pre_pwm_L)&&(pre_pwm_R) > 0){
58340500022 0:18fcf04b703b 139 L_NA = 0;
58340500022 0:18fcf04b703b 140 L_NB = 1;
58340500022 0:18fcf04b703b 141 //
58340500022 0:18fcf04b703b 142 R_NA = 0;
58340500022 0:18fcf04b703b 143 R_NB = 1;
58340500022 0:18fcf04b703b 144
58340500022 0:18fcf04b703b 145 }
58340500022 0:18fcf04b703b 146 else if((pre_pwm_L)&&(pre_pwm_R) < 0){
58340500022 0:18fcf04b703b 147 L_NA = 1;
58340500022 0:18fcf04b703b 148 L_NB = 0;
58340500022 0:18fcf04b703b 149 //
58340500022 0:18fcf04b703b 150 R_NA = 1;
58340500022 0:18fcf04b703b 151 R_NB = 0;
58340500022 0:18fcf04b703b 152 }
58340500022 0:18fcf04b703b 153
58340500022 0:18fcf04b703b 154 mypwm_L = abs(pre_pwm_L);
58340500022 0:18fcf04b703b 155 mypwm_R = abs(pre_pwm_R);
58340500022 0:18fcf04b703b 156 //****recheck******
58340500022 0:18fcf04b703b 157 // raw_test[0] = pre_pwm_L;
58340500022 0:18fcf04b703b 158 // raw_test[1] = pre_pwm_R;
58340500022 0:18fcf04b703b 159 // x_line_msg.data = raw_test;
58340500022 0:18fcf04b703b 160 // mbed_send.publish(&x_line_msg);
58340500022 0:18fcf04b703b 161 // nh.spinOnce();
58340500022 0:18fcf04b703b 162
58340500022 0:18fcf04b703b 163 }
58340500022 0:18fcf04b703b 164
58340500022 0:18fcf04b703b 165 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
58340500022 0:18fcf04b703b 166
58340500022 0:18fcf04b703b 167 int main() {
58340500022 0:18fcf04b703b 168
58340500022 0:18fcf04b703b 169 //................
58340500022 0:18fcf04b703b 170 nh.initNode();
58340500022 0:18fcf04b703b 171 nh.advertise(chatter_test);
58340500022 0:18fcf04b703b 172 nh.advertise(mbed_send);
58340500022 0:18fcf04b703b 173 nh.subscribe(sub);
58340500022 0:18fcf04b703b 174 //Array_msg.layout.dim_length = 1;
58340500022 0:18fcf04b703b 175 Array_msg.data_length = 2;
58340500022 0:18fcf04b703b 176 //
58340500022 0:18fcf04b703b 177 x_line_msg.data_length = 2;
58340500022 0:18fcf04b703b 178 //................
58340500022 0:18fcf04b703b 179 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 180 Send_data.attach(&Send_V_to_ros,0.05);
58340500022 0:18fcf04b703b 181
58340500022 0:18fcf04b703b 182 mypwm_L.period(0.0005);
58340500022 0:18fcf04b703b 183 mypwm_R.period(0.0005);
58340500022 0:18fcf04b703b 184 // mypwm_L = 0;
58340500022 0:18fcf04b703b 185 // mypwm_R = 0;
Tarzoozoo 1:5c48d64f17cb 186 led = 1;
58340500022 0:18fcf04b703b 187 while (1) {
58340500022 0:18fcf04b703b 188 nh.spinOnce();
58340500022 0:18fcf04b703b 189 wait_ms(1);
58340500022 0:18fcf04b703b 190 }
58340500022 0:18fcf04b703b 191
58340500022 0:18fcf04b703b 192 }