File content as of revision 5:e9a3ee77cb97:

#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>
//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};
// -> 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;
}
void baseCallback(const std_msgs::Float32MultiArray& base_msg){
    OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(base_msg.data[1], base_msg.data[0], base_msg.data[2]);
    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]);
    }
}

void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
    myled = !myled;   // blink the led
}

ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
int main() {
    nh.getHardware()->setBaud(BAUD);
    nh.initNode();
    nh.subscribe(subax);
    nh.subscribe(subbt);
    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
    }
    
}