File content as of revision 0:54452c8078db:

#define BAUD 115200
#include "mbed.h"
#include "math.h"
//#include "rtos.h"
#include <ros.h>
#include "std_msgs/Bool.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"


using namespace ros;
using namespace std_msgs;
using namespace geometry_msgs;


NodeHandle nh;

DigitalOut myled = LED1;
// -> Motor PWN Pin
PwmOut motor_pwm[] = {D6, D5, D4, D3};
DigitalOut LED01 = D7;
DigitalOut LED02 = D8;
DigitalOut LED03 = D9;
//motor number

void antiturning(float a, float b){
    motor_pwm[0]=0;
    motor_pwm[1]=a;
    motor_pwm[2]=0;
    motor_pwm[3]=b;
}

void turning(float a, float b){
    motor_pwm[0]=a;
    motor_pwm[1]=0;
    motor_pwm[2]=b;
    motor_pwm[3]=0;
}

void r1Callback(const Bool& _msg){
    bool check = _msg.data;
    if(check){
        LED01 = 1;
        LED02 = 1;
        LED03 = 1;}
    else{
        LED01 = 0;
        LED02 = 0;
        LED03 = 0;}
}

void twCallback(const Twist& _msg) {
    float x = _msg.linear.x;
    float y = _msg.linear.y;
    
    if (x==0.0){
        if(y<0){
            antiturning(abs(y), abs(y));
        }
        else{
            turning(y, y);
        }
    }
    else{
        float angle = atan(abs(y)/abs(x));
        float angle45 = atan(1.0);
        if(x<0){
            if(angle <= angle45){
                motor_pwm[0]=0;
                motor_pwm[1]=abs(x + abs(y));
                motor_pwm[2]=-x;
                motor_pwm[3]=0;
                
            }
            else{
                if(y<0){
                    antiturning(-y, -y+x);}
                else{
                    turning(y+x, y);}
            }
        }
        else {
            if(angle <= angle45){
                motor_pwm[0]=x;
                motor_pwm[1]=0;
                motor_pwm[2]=0;
                motor_pwm[3]=x - abs(y);
                
            }
            else{
                if(y<0){
                    antiturning(-y-x,-y);}
                else{
                    turning(y, y-x);}
            }
        }
    }
//    if(y<0)
//    {
//        motor_pwm[2]=0;
//        motor_pwm[3]=abs(y);
//    }
//    else
//    {
//        motor_pwm[2]=y;
//        motor_pwm[3]=0;
//    }
//   if(x<0)
//   {
//       motor_pwm[0]=0;
//       motor_pwm[1]=abs(x);
//       
//   }
//   else
//   {
//       motor_pwm[0]=x;
//       motor_pwm[1]=0;
//   }

}

Subscriber<Twist> subtw("base_twist", &twCallback);
Subscriber<Bool> subr1("bt_r1", &r1Callback);
int main() {
    //Rate rate(10);
    nh.getHardware()->setBaud(BAUD);
    nh.initNode();

    nh.subscribe(subtw);
    nh.subscribe(subr1);
    
    motor_pwm[0]=0;
    motor_pwm[1]=0;
    motor_pwm[2]=0;
    motor_pwm[3]=0;

    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
    }
    
}