File content as of revision 1:782534ae7166:

#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;
Thread battery_thread;

AnalogIn battery(A0);

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

void turn(float r, float l){
    float a, b, c, d;
    if (l < 0){
        a = 0.0;
        b = -l;    
    }
    else{
        a = l;
        b = 0.0;
    }
    if (r < 0){
        c = 0.0;
        d = -r;
    }
    else{
        c = r;
        d = 0.0;
    }    
    motor_pwm[0]=a;
    motor_pwm[1]=b;
    motor_pwm[2]=c;
    motor_pwm[3]=d;
}

void resize(float &a, float &b){
    if (a != 0 or b != 0) {
        if (b > a){
            a = a/b;
            if (b > 1) 
                b = 1;
        }
        else{
            b = b/a;
            if (a > 1)
                a = 1;
        }
    }
    
}
 
void r1Callback(const Bool& _msg){
    bool check = _msg.data;
    if(check){
        LED01 = 1;
        LED02 = 1;
        LED03 = 1;}
    else{
        LED01 = 0;
        LED02 = 0;
        LED03 = 0;}
}

float sum = 0.0;
//std_msgs::String motor_msg;
//char buffer[50];
//Publisher motor_pub("motor", &motor_msg);


void twCallback(const Twist& _msg) {
    float x = _msg.linear.x;
    float y = _msg.linear.y;
    sum =abs(x)+abs(y);
    resize(y, sum);
    if (y < 0)
        sum = -sum;
    if (x < 0)
        turn(y, sum);
    else
        turn(sum, y);

//    sprintf(buffer, "turn: %f, %f", sum, y);
//    motor_msg.data = buffer;
//    motor_pub.publish(&motor_msg);
    
//
//    if (x==0.0){
//        turn(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);}
//            }
//        }
//    }

}
std_msgs::String str_msg;
Publisher pub("battery", &str_msg);

Subscriber<Twist> subtw("base_twist", &twCallback);
Subscriber<Bool> subr1("bt_r1", &r1Callback);

void battery_pub(){
    char buf[10];
    while(1){
        
        sprintf(buf, "%d", int(((battery.read()*3.3f) - (6/2.68))/(2.4/268)));
//        sprintf(buf,"%f",battery.read());
        str_msg.data = buf;
        pub.publish(&str_msg);
//        nh.spinOnce();
        Thread::wait(1000);
        
    }
}

int main() {
    //Rate rate(10);
    nh.getHardware()->setBaud(BAUD);
    nh.initNode();
    nh.advertise(pub);
//    nh.advertise(motor_pub);
    nh.subscribe(subtw);
    nh.subscribe(subr1);
    
    motor_pwm[0]=0;
    motor_pwm[1]=0;
    motor_pwm[2]=0;
    motor_pwm[3]=0;
    
    battery_thread.start(battery_pub);
    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
    }
    
}