John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

main.cpp@6:66de1f4abff4, 2021-02-17 (annotated)

Committer:
howanglam3
Date:
Wed Feb 17 09:46:01 2021 +0000
Revision:
6:66de1f4abff4
Parent:
5:e9a3ee77cb97
Child:
7:fe1f52d848fd
add sub to twist

Who changed what in which revision?

UserRevisionLine numberNew contents of line
howanglam3 3:a1a69ae50c18 1 #define BAUD 115200
Gary Servin 0:f48f597ef690 2 /*
Gary Servin 0:f48f597ef690 3 * rosserial Subscriber Example
Gary Servin 0:f48f597ef690 4 * Blinks an LED on callback
Gary Servin 0:f48f597ef690 5 */
Gary Servin 0:f48f597ef690 6 #include "mbed.h"
Gary Servin 0:f48f597ef690 7 #include <ros.h>
howanglam3 3:a1a69ae50c18 8 #include <std_msgs/Float32MultiArray.h>
howanglam3 3:a1a69ae50c18 9 #include <std_msgs/Int32MultiArray.h>
howanglam3 6:66de1f4abff4 10 #include "geometry_msgs/Twist.h"
howanglam3 4:aa8ef06b9469 11 //motor header
howanglam3 4:aa8ef06b9469 12 #include "MotorDrive.h"
howanglam3 4:aa8ef06b9469 13 #include "OmniWheel.h"
Gary Servin 0:f48f597ef690 14
Gary Servin 0:f48f597ef690 15 ros::NodeHandle nh;
howanglam3 4:aa8ef06b9469 16
Gary Servin 0:f48f597ef690 17 DigitalOut myled(LED1);
howanglam3 4:aa8ef06b9469 18 DigitalOut Refer_Volt_3_3 = PB_1;
howanglam3 4:aa8ef06b9469 19 DigitalOut motor_enable = PC_7;
howanglam3 4:aa8ef06b9469 20 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
howanglam3 4:aa8ef06b9469 21 // -> Motor PWN Pin
howanglam3 4:aa8ef06b9469 22 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
howanglam3 4:aa8ef06b9469 23 //motor number
howanglam3 4:aa8ef06b9469 24 const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
howanglam3 4:aa8ef06b9469 25 //motor pwm period (ms)
howanglam3 4:aa8ef06b9469 26 const int pwm_period = 20;
howanglam3 4:aa8ef06b9469 27 //motor stop pwm
howanglam3 4:aa8ef06b9469 28 const double stop_pwm = 0.5;
howanglam3 4:aa8ef06b9469 29
howanglam3 4:aa8ef06b9469 30 MotorDrive motor_drive;
howanglam3 4:aa8ef06b9469 31
howanglam3 4:aa8ef06b9469 32 void motor_init() {
howanglam3 4:aa8ef06b9469 33 //init motor
howanglam3 4:aa8ef06b9469 34 motor_enable = 0;
howanglam3 4:aa8ef06b9469 35
howanglam3 4:aa8ef06b9469 36 for(int i = 0; i < motor_num; i++){
howanglam3 4:aa8ef06b9469 37 motor_pwm[i].period_ms(pwm_period);
howanglam3 4:aa8ef06b9469 38 motor_pwm[i] = stop_pwm;
howanglam3 4:aa8ef06b9469 39 }
howanglam3 4:aa8ef06b9469 40 //ref voltage
howanglam3 4:aa8ef06b9469 41 Refer_Volt_3_3 = 1;
howanglam3 4:aa8ef06b9469 42 wait(1);
howanglam3 4:aa8ef06b9469 43 motor_enable = 1;
howanglam3 4:aa8ef06b9469 44 }
howanglam3 4:aa8ef06b9469 45
howanglam3 4:aa8ef06b9469 46 void motorMove(int motor_index, double pwm){
howanglam3 5:e9a3ee77cb97 47 motor_stop[motor_index] = 0;
howanglam3 4:aa8ef06b9469 48 motor_pwm[motor_index] = pwm;
howanglam3 4:aa8ef06b9469 49 }
howanglam3 5:e9a3ee77cb97 50 void motorStop(int motor_index){
howanglam3 5:e9a3ee77cb97 51 motor_stop[motor_index] = 1;
howanglam3 5:e9a3ee77cb97 52 motor_pwm[motor_index] = 0.5;
howanglam3 5:e9a3ee77cb97 53 }
howanglam3 6:66de1f4abff4 54 float round(float x){
howanglam3 6:66de1f4abff4 55 float value = (int)(x*100+0.5);
howanglam3 6:66de1f4abff4 56 return (float)value/100;
howanglam3 6:66de1f4abff4 57 }
howanglam3 4:aa8ef06b9469 58 void baseCallback(const std_msgs::Float32MultiArray& base_msg){
howanglam3 6:66de1f4abff4 59 double x = (double)round(base_msg.data[1]);
howanglam3 6:66de1f4abff4 60 double y = (double)round(base_msg.data[0]);
howanglam3 6:66de1f4abff4 61 double z = (double)round(base_msg.data[2]);
howanglam3 6:66de1f4abff4 62 OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(x, y, z);
howanglam3 4:aa8ef06b9469 63 vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();
howanglam3 6:66de1f4abff4 64 //for(int i = 0; i < motor_num; i++){
howanglam3 6:66de1f4abff4 65 // if(omni_wheel_pwm[i]==0.5)
howanglam3 6:66de1f4abff4 66 // motorStop(i);
howanglam3 6:66de1f4abff4 67 // else
howanglam3 6:66de1f4abff4 68 // motorMove(i, omni_wheel_pwm[i]);
howanglam3 6:66de1f4abff4 69 // }
howanglam3 6:66de1f4abff4 70
howanglam3 6:66de1f4abff4 71 if(omni_wheel_pwm[0] == 0.5)
howanglam3 6:66de1f4abff4 72 motorStop(0);
howanglam3 6:66de1f4abff4 73 else
howanglam3 6:66de1f4abff4 74 motorMove(0, omni_wheel_pwm[0]);
howanglam3 6:66de1f4abff4 75
howanglam3 6:66de1f4abff4 76 if(omni_wheel_pwm[1] == 0.5)
howanglam3 6:66de1f4abff4 77 motorStop(1);
howanglam3 6:66de1f4abff4 78 else
howanglam3 6:66de1f4abff4 79 motorMove(1, omni_wheel_pwm[1]);
howanglam3 6:66de1f4abff4 80
howanglam3 6:66de1f4abff4 81 if(omni_wheel_pwm[2] == 0.5)
howanglam3 6:66de1f4abff4 82 motorStop(2);
howanglam3 6:66de1f4abff4 83 else
howanglam3 6:66de1f4abff4 84 motorMove(2, omni_wheel_pwm[2]);
howanglam3 4:aa8ef06b9469 85 }
Gary Servin 0:f48f597ef690 86
howanglam3 3:a1a69ae50c18 87 void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
Gary Servin 0:f48f597ef690 88 myled = !myled; // blink the led
Gary Servin 0:f48f597ef690 89 }
Gary Servin 0:f48f597ef690 90
howanglam3 6:66de1f4abff4 91 void twCallback(const geometry_msgs::Twist& _msg) {
howanglam3 6:66de1f4abff4 92 OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z);
howanglam3 6:66de1f4abff4 93
howanglam3 6:66de1f4abff4 94 vector<double> omni_wheel_pwm = omni_wheel_data.getPwm(); // Get PWM data
howanglam3 6:66de1f4abff4 95
howanglam3 6:66de1f4abff4 96 for(int i = 0; i < motor_num; i++) // Send PWM to each motor
howanglam3 6:66de1f4abff4 97 if(omni_wheel_pwm[i] != stop_pwm)
howanglam3 6:66de1f4abff4 98 motorMove(i, omni_wheel_pwm[i]);
howanglam3 6:66de1f4abff4 99 else
howanglam3 6:66de1f4abff4 100 motorStop(i);
howanglam3 6:66de1f4abff4 101 }
howanglam3 6:66de1f4abff4 102
howanglam3 4:aa8ef06b9469 103 ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
howanglam3 3:a1a69ae50c18 104 ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
howanglam3 6:66de1f4abff4 105 ros::Subscriber<geometry_msgs::Twist> subtw("base_twist", &twCallback);
Gary Servin 0:f48f597ef690 106 int main() {
howanglam3 3:a1a69ae50c18 107 nh.getHardware()->setBaud(BAUD);
Gary Servin 0:f48f597ef690 108 nh.initNode();
howanglam3 6:66de1f4abff4 109 // nh.subscribe(subax);
howanglam3 3:a1a69ae50c18 110 nh.subscribe(subbt);
howanglam3 4:aa8ef06b9469 111 motor_init();
Gary Servin 0:f48f597ef690 112 while (1) {
howanglam3 3:a1a69ae50c18 113 nh.spinOnce();
howanglam3 4:aa8ef06b9469 114
howanglam3 4:aa8ef06b9469 115
howanglam3 4:aa8ef06b9469 116 if(nh.connected())
howanglam3 4:aa8ef06b9469 117 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
howanglam3 4:aa8ef06b9469 118 else
howanglam3 4:aa8ef06b9469 119 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
Gary Servin 0:f48f597ef690 120 }
howanglam3 4:aa8ef06b9469 121
Gary Servin 0:f48f597ef690 122 }