![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
carbase
Dependencies: mbed mbed-rtos ros_lib_melodic
main.cpp
- Committer:
- howanglam3
- Date:
- 2021-02-18
- Revision:
- 7:fe1f52d848fd
- Parent:
- 6:66de1f4abff4
- Child:
- 8:e542adce95e9
File content as of revision 7:fe1f52d848fd:
#define BAUD 115200 /* * rosserial Subscriber Example * Blinks an LED on callback */ #include "mbed.h" #include <ros.h> #include <std_msgs/Float32MultiArray.h> #include <std_msgs/Int32MultiArray.h> #include "geometry_msgs/Twist.h" //motor header #include "MotorDrive.h" #include "OmniWheel.h" ros::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}; geometry_msgs::Twist wheel_data; // -> Motor PWN Pin PwmOut motor_pwm[] = {PA_9, PC_8, PA_11}; //motor number const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut); //motor pwm period (ms) const int pwm_period = 20; //motor stop pwm const double stop_pwm = 0.5; MotorDrive motor_drive; void motor_init() { //init motor motor_enable = 0; for(int i = 0; i < motor_num; i++){ motor_pwm[i].period_ms(pwm_period); motor_pwm[i] = stop_pwm; } //ref voltage Refer_Volt_3_3 = 1; wait(1); motor_enable = 1; } void motorMove(int motor_index, double pwm){ motor_stop[motor_index] = 0; motor_pwm[motor_index] = pwm; } void motorStop(int motor_index){ motor_stop[motor_index] = 1; motor_pwm[motor_index] = 0.5; } float round(float x){ float value = (int)(x*100+0.5); return (float)value/100; } void baseCallback(const std_msgs::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]); OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(x, y, z); vector<double> omni_wheel_pwm = omni_wheel_data.getPwm(); //for(int i = 0; i < motor_num; i++){ // if(omni_wheel_pwm[i]==0.5) // motorStop(i); // else // motorMove(i, omni_wheel_pwm[i]); // } if(omni_wheel_pwm[0] == 0.5) motorStop(0); else motorMove(0, omni_wheel_pwm[0]); if(omni_wheel_pwm[1] == 0.5) motorStop(1); else motorMove(1, omni_wheel_pwm[1]); if(omni_wheel_pwm[2] == 0.5) motorStop(2); else motorMove(2, omni_wheel_pwm[2]); } void messageCb(const std_msgs::Float32MultiArray& toggle_msg){ myled = !myled; // blink the led } void twCallback(const geometry_msgs::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 for(int i = 0; i < motor_num; i++) // Send PWM to each motor if(omni_wheel_pwm[i] != stop_pwm) motorMove(i, omni_wheel_pwm[i]); else motorStop(i); } void Callback(const geometry_msgs::Twist& _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", &twCallback); int main() { //ros::Rate rate(10); nh.getHardware()->setBaud(BAUD); nh.initNode(); // nh.subscribe(subax); nh.subscribe(subbt); nh.subscribe(subtw); motor_init(); while (1) { nh.spinOnce(); if(nh.connected()) myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson else myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson } }