File content as of revision 9:bfd88f814eec:

#define BAUD 115200
/*
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */
#include "mbed.h"
#include "rtos.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"

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
Twist wheel_data; //global
Thread wheel_calc;
Twist save;

// -> 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 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 Float32MultiArray& toggle_msg){
    myled = !myled;   // blink the led
}

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
    
    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 wheel_calc_thread() {
    while (true) {
        
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 Twist& _msg) {
    wheel_data = _msg;
}

Subscriber<Float32MultiArray> subax("moving_base", &baseCallback);
Subscriber<Float32MultiArray> subbt("fcn_button", &messageCb);
Subscriber<Twist> subtw("base_twist", &saveCallback);
int main() {
    //Rate rate(10);
    nh.getHardware()->setBaud(BAUD);
    nh.initNode();
    wheel_calc.start(wheel_calc_thread);
//    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
    }
    
}