![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
carbase
Dependencies: mbed mbed-rtos ros_lib_melodic
Diff: main.cpp
- Revision:
- 9:bfd88f814eec
- Parent:
- 8:e542adce95e9
--- a/main.cpp Thu Feb 18 10:15:52 2021 +0000 +++ b/main.cpp Thu Feb 25 05:54:25 2021 +0000 @@ -13,16 +13,23 @@ #include "MotorDrive.h" #include "OmniWheel.h" -ros::NodeHandle nh; +using namespace ros; +using namespace std_msgs; +using namespace geometry_msgs; + + +NodeHandle nh; DigitalOut myled(LED1); DigitalOut Refer_Volt_3_3 = PB_1; DigitalOut motor_enable = PC_7; DigitalOut motor_stop[] = {PB_6, PC_5, PC_0}; + // thread handling wheel -geometry_msgs::Twist wheel_data; //global +Twist wheel_data; //global Thread wheel_calc; +Twist save; // -> Motor PWN Pin PwmOut motor_pwm[] = {PA_9, PC_8, PA_11}; @@ -61,7 +68,7 @@ float value = (int)(x*100+0.5); return (float)value/100; } -void baseCallback(const std_msgs::Float32MultiArray& base_msg){ +void baseCallback(const Float32MultiArray& base_msg){ double x = (double)round(base_msg.data[1]); double y = (double)round(base_msg.data[0]); double z = (double)round(base_msg.data[2]); @@ -90,11 +97,11 @@ motorMove(2, omni_wheel_pwm[2]); } -void messageCb(const std_msgs::Float32MultiArray& toggle_msg){ +void messageCb(const Float32MultiArray& toggle_msg){ myled = !myled; // blink the led } -void twCallback(const geometry_msgs::Twist& _msg) { +void twCallback(const Twist& _msg) { OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z); vector<double> omni_wheel_pwm = omni_wheel_data.getPwm(); // Get PWM data @@ -108,21 +115,31 @@ void wheel_calc_thread() { while (true) { - // calc - delay_for(5); + +bool x = wheel_data.linear.x==save.linear.x; +bool y = wheel_data.linear.y==save.linear.y; +bool z = wheel_data.angular.z==save.angular.z; + +// calc + if(!(x && y && z)) + { + twCallback(wheel_data); + wait(5); + } + save = wheel_data; } } -void saveCallback(const geometry_msgs::Twist& _msg) { +void saveCallback(const Twist& _msg) { wheel_data = _msg; } -ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback); -ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb); -ros::Subscriber<geometry_msgs::Twist> subtw("base_twist", &saveCallback); +Subscriber<Float32MultiArray> subax("moving_base", &baseCallback); +Subscriber<Float32MultiArray> subbt("fcn_button", &messageCb); +Subscriber<Twist> subtw("base_twist", &saveCallback); int main() { - //ros::Rate rate(10); + //Rate rate(10); nh.getHardware()->setBaud(BAUD); nh.initNode(); wheel_calc.start(wheel_calc_thread);