File content as of revision 1:fbdbf4ac12c6:

    // Hello World to sweep a servo through its full range
// ROS Connect Baud Rate 
#define BAUD_RATE 115200

#include "mbed.h"
#include "Servo.h"
#include <ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
#include <std_msgs/Int16MultiArray.h>
ros::NodeHandle nh;
DigitalOut myled(LED1);
void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
void msgCb_button(const std_msgs::Int8 &button_msg);

ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);

std_msgs::String str_msg;
ros::Publisher responser("display", &str_msg);

char msg[200] = {0};

DigitalIn button(USER_BUTTON);

DigitalOut up = D12; // pin 1 of faulhaber
DigitalOut down = A5; // pin 2 of faulhaber
DigitalOut left = D2; // DC motor 1 (via L298N control pin 1)
DigitalOut right = D4; // DC motor 2 (via L298N control pin 2)
//AnalogOut left = A0; // DC motor 1 (via L298N control pin 1)
//AnalogOut right = A1; // DC motor 2 (via L298N control pin 2)
DigitalOut lock = D14; // 電磁石
DigitalOut back = A4; // pin 3 of faulhaber
DigitalOut eject_1 = D10; // valve 1
DigitalOut eject_2 = D5; // valve 2
//DigitalOut savePos = D0; savepos by back=1 & up=1 & down=1
DigitalIn firing = D3;

 
void itoa(int num, char *str, int radix)
{ /*索引表*/
    char index[] = "0123456789ABCDEF";
    unsigned unum; /*中间变量*/
    int i = 0, j, k;
    /*确定unum的值*/
    if (radix == 10 && num < 0) /*十进制负数*/
    {
        unum = (unsigned)-num;
        str[i++] = '-';
    }
    else
        unum = (unsigned)num; /*其他情况*/
    /*转换*/
    do
    {
        str[i++] = index[unum % (unsigned)radix];
        unum /= radix;
    } while (unum);
    str[i] = '\0';
    /*逆序*/
    if (str[0] == '-')
        k = 1; /*十进制负数*/
    else
        k = 0;

    for (j = k; j <= (i - 1) / 2; j++)
    {
        char temp;
        temp = str[j];
        str[j] = str[i - 1 + k - j];
        str[i - 1 + k - j] = temp;
    }
}

/*
* joystick_msg: (Left/Right, x/y, value)   
*/

void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg)
{
//    myled = !myled; // blink the led
    memset(msg, 0, sizeof(msg));
    // eg. Received (0,0,-100)
    strcpy(msg, "Received (");
    int len = strlen(msg);
    msg[len++] = '0' + joystick_msg.data[0];
    msg[len++] = ',';
    itoa(joystick_msg.data[1], msg + len, 10);
    len = strlen(msg);
    msg[len++] = ',';
    itoa(joystick_msg.data[2], msg + len, 10);
    len = strlen(msg);
    msg[len++] = ')';
    msg[len++] = ';';
    msg[len++] = ' ';

    // Elaboration of the message
    // -----------L3------------
    // x
    if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0)
    {
        strcpy(msg + len, "L3: direction is x(horizontal) and the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
    }
    // y
    if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1)
    {
        strcpy(msg + len, "L3: direction is y(vertical) and the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
    }

    // -----------R3------------
    // x
    if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0)
    {
        strcpy(msg + len, "R3: direction is x(horizontal) and the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
    }
    // y
    if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1)
    {
        strcpy(msg + len, "R3: direction is y(vertical) and the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
    }

    // -----------L2------------
    if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1)
    {
        strcpy(msg + len, "L2: the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
    }
    // -----------R2------------
    if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
    {
        strcpy(msg + len, "L3: the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
    }

    responser.publish(&str_msg);
}

// button_msg: Int8
void msgCb_button(const std_msgs::Int8 &button_msg)
{
//    myled = !myled; // blink the led
    memset(msg, 0, sizeof(msg));
    strcpy(msg, "Received ");
    itoa(button_msg.data, msg + 9, 10);
    //    int len = my_strlen(msg);
    int len = strlen(msg);
    msg[len++] = ';';
    msg[len++] = ' ';
    
    switch (button_msg.data)
    {
    case 1:
        eject_1 = 1 - eject_1;
        eject_2 = 1 - eject_2;
        strcpy(msg + len, "x is pressed, eject_1 is ");
        msg[strlen(msg)] = '0' + int(eject_1);
        break;
    case -1:
//        eject = 0;
        strcpy(msg + len, "x is released, eject_2 is ");
        msg[strlen(msg)] = '0' + int(eject_2);
        break;
    case 2:
        lock = 1 - lock;
        strcpy(msg + len, "o is pressed, lock is: ");
        msg[strlen(msg)] = '0' + int(lock);
        
        break;
    case -2:
        strcpy(msg + len, "o is released");
        break;
    case 3:
        back  = 1;
        strcpy(msg + len, "triangle is pressed, back is ");
        msg[strlen(msg)] = '0' + int(back);
        break;
    case -3:
        back = 0; 
        strcpy(msg + len, "triangle is released, back is ");
        msg[strlen(msg)] = '0' + int(back);
        break;
    case 4:
        strcpy(msg + len, "square is pressed");
        up = 1; down = 1;
        break;
    case -4:
        strcpy(msg + len, "square is released");
        up = 0; down = 0;
        break;
    case 5:
        down = 1;
        up = 0;
        strcpy(msg + len, "down arrow is pressed, down is ");
        msg[strlen(msg)] = '0' + int(down);
        break;
    case -5:
        down = 0;
        strcpy(msg + len, "down arrow is released, down is ");
        msg[strlen(msg)] = '0' + int(down);
        break;
    case 6:
        left = 0; right = 1;
        strcpy(msg + len, "right arrow is pressed, right is ");
        msg[strlen(msg)] = '0' + int(right);
        break;
    case -6:
        right = 0;
        strcpy(msg + len, "right arrow is released, right is ");
        msg[strlen(msg)] = '0' + int(right);
        break;
    case 7:
        up = 1;
        down = 0;
        strcpy(msg + len, "up arrow is pressed, up is");
        msg[strlen(msg)] = '0' + int(up);
        break;
    case -7:
        up = 0;
        strcpy(msg + len, "up arrow is released, up is");
        msg[strlen(msg)] = '0' + int(up);
        break;
    case 8:
        left = 1; right = 0;
        strcpy(msg + len, "left arrow is pressed, left is ");
        msg[strlen(msg)] = '0' + int(left);
        break;
    case -8:
        left = 0;
        strcpy(msg + len, "left arrow is released, left is ");
        msg[strlen(msg)] = '0' + int(left);
        break;
    case 9:
        strcpy(msg + len, "L1 is pressed, left is ");
        msg[strlen(msg)] = '0' + int(left);
        break;
    case -9:
        strcpy(msg + len, "L1 is released");
        break;
    case 10:
        strcpy(msg + len, "R1 is pressed");
        break;
    case -10:
        strcpy(msg + len, "R1 is released");
        break;
    case 11:
        strcpy(msg + len, "share is pressed");
        break;
    case -11:
        strcpy(msg + len, "share is released");
        break;
    case 12:
        strcpy(msg + len, "option is pressed");
        break;
    case -12:
        strcpy(msg + len, "option is released");
        break;
    case 13:
        up = 1; down = 1; back = 1;
        strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
        break;
    case -13:
        up = 0; down = 0; back= 0;
        strcpy(msg + len, "playstation button is released");
        break;
    default:
        len = strlen(msg);
        strcpy(msg + len, "Unrecognized pattern");
        break;
    }

    responser.publish(&str_msg);
}
int main() 
{  
    // -> Set Baud Rate
    nh.getHardware()->setBaud(BAUD_RATE);
    nh.initNode();
    nh.advertise(responser);
    str_msg.data = msg;
    responser.publish(&str_msg);
    nh.subscribe(sub_button);
    nh.subscribe(sub_joystick);
    myled = 0;
    back = 0;up = 0; down = 0; lock = 0; left = 0; right = 0;
    eject_1 = 0; eject_2 = 1;
    strcpy(msg, "Byebye world.");
    responser.publish(&str_msg);
    int start = 1;
    firing.mode(PullDown);
    // Main programming
    while(true) {
        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
        if(button == 1)
        {
            myled = 0;
            down = 0; up = 1; back = 1;
            start = 1;
            while(button == 1);
        }
        if(firing.read() == 1 && start == 1)
        {
            up = 0;
            down = 1; 
            back = 1;
            while(firing.read() == 1);
            wait(0.3);
            down = 0; back = 0; up = 0;
            start = 0;
            
        }
    }
}