File content as of revision 2:e570143bf35e:

// 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>
#include "DC_Motor_Controller.h"

// Declare Functions
void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
void msgCb_button(const std_msgs::Int8 &button_msg);
void itoa(int num, char *str, int radix);

// Define ros variables
ros::NodeHandle nh;
// Subs
ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);
// Pubs
std_msgs::String str_msg;
ros::Publisher responser("display", &str_msg);

// ============  Define IOs  =============

// External Button
DigitalIn button(USER_BUTTON);
// LED
DigitalOut myled(LED1);
// Faulhaber pins
DigitalOut up = A3;  // pin 1 of faulhaber
DigitalOut down = A5; // pin 2 of faulhaber
DigitalOut back = A4; // pin 3 of faulhaber
// Electromagnet
DigitalOut lock = D14; 
// Firing pin
DigitalIn firing1 = D3; // Reset Faulhaber
DigitalIn firing2 = D15; // Load 1 arrow
DigitalIn firing3 = D2; // Pick, small
// Valves
DigitalOut eject_1 = A1; // valve 1
DigitalOut eject_2 = A2;  // valve 2
// DC motors
DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; 轉夾
DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; load 1 arrow

// Global Control variable
char msg[200] = {0}; // msg for responser
int stage_pick = 0;  // stage of picking up arrows from the rack
int n_loaded = 0;  // number of arrows. 
int faul_reset = 0;
int load_manual = 1; // tune the loading 1 arrow manually

// button_msg: Int8
void msgCb_button(const std_msgs::Int8 &button_msg)
{
    memset(msg, 0, sizeof(msg));
    strcpy(msg, "Received ");
    itoa(button_msg.data, msg + 9, 10);
    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:
        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:
        load_manual = 0;
        strcpy(msg + len, "right arrow is pressed. ");
        msg[strlen(msg)] = '0' + n_loaded;
        dc_motor1.set_out(0,1);    
        n_loaded++;  
        if(firing2 == 1)
        {
            if(n_loaded <= 5)
            {
                dc_motor1.set_out(0,0.5);
                wait(1.2);                
            }
            else
            {
                strcpy(msg + len, "right arrow is pressed. ");
                strcpy(msg + strlen(msg), "n_loaded cleared!");
                n_loaded = 0;
            }
            dc_motor1.set_out(0,0);
        }
//        load_manual = 1;
        break;
    case -6:
        
        strcpy(msg + len, "right arrow is released. ");
        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:
        strcpy(msg + len, "left arrow is pressed, left is ");
        msg[strlen(msg)] = '0' + stage_pick;
        if(stage_pick == 0)
        {
            dc_motor2.set_out(0, 0.5);
            if(firing3 == 1)
                dc_motor2.set_out(0,0);
            stage_pick++;
            eject_1 = 1; eject_2 = 0;
        }
        else if(stage_pick == 1)
        {
            dc_motor2.set_out(0.5,0);
            wait(0.3);
            dc_motor2.set_out(0,0);
            stage_pick++;
        }
        else if(stage_pick == 2)
        {
            dc_motor2.set_out(0.5,0);
            wait(0.3);
            dc_motor2.set_out(0,0);
            stage_pick = 0;
        }
        break;
    case -8:
        strcpy(msg + len, "left arrow is released, left is ");
        break;
    case 9:
        strcpy(msg + len, "L1 is pressed");
        break;
    case -9:
        strcpy(msg + len, "L1 is released");
        break;
    case 10:
        dc_motor1.set_out(0,0);
        dc_motor2.set_out(0,0);
        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);
}


// --------------- main -----------------
int main() 
{  
    
    // -> Set Baud Rate
    nh.getHardware()->setBaud(BAUD_RATE);
    nh.initNode();
    // Advertise publishers
    nh.advertise(responser);
    // Initiate display
    str_msg.data = msg;
//    responser.publish(&str_msg);
    // Initiate subscribers
    nh.subscribe(sub_button);
    nh.subscribe(sub_joystick);
    // Initiate IOs, keep everything at rest
    myled = 0;
    back = 0;up = 0; down = 0; lock = 0;  
    eject_1 = 0; eject_2 = 1;
    
    // Initiate firing pins
    firing1.mode(PullUp); // default 1
    firing2.mode(PullDown); //default 0
    firing3.mode(PullUp); // default 1
    
    // Reset dc motor
    dc_motor1.reset();
    dc_motor1.set_pid(0.008, 0, 0, 0.0);
    dc_motor2.reset();
    dc_motor2.set_pid(0.008, 0, 0, 0.0);
//    dc_motor2.set_out(0,0.5);
//    wait(0.3);
//    dc_motor2.move_angle(100);
    dc_motor2.set_out(0,0);
//     dc_motor1.move_angle(100);
//    dc_motor1.set_out(0.5,0); // -> testing only
    
    // Main programming
    int start = 0; // control variable for resetting shooter
    int tmp = 0;

    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;
            if(!faul_reset)
            {
                down = 0; up = 1; back = 1; // 101
                start = 1;
                faul_reset = 1;
            }
            else
            {
                dc_motor1.set_out(0,1);   
            }
            while(button == 1);
            wait(0.2);
            memset(msg, 0, sizeof(msg));
            strcpy(msg, "user button is pressed, faul_reset is ");
            msg[strlen(msg)] = '0' + faul_reset;            
            responser.publish(&str_msg);
        }
        if(load_manual && firing2 == 1)
        {
            dc_motor1.set_out(0,0);   
        }
        if(firing1.read() == 0 && start == 1)
        {
            up = 0; down = 1; back = 1; // 110
            while(firing1.read() == 0);
            wait(0.3);
            down = 0; back = 0; up = 0;
            start = 0;
            memset(msg, 0, sizeof(msg));
            strcpy(msg, "firing1 is touched: ");
            msg[strlen(msg)] = '0' + firing1.read();
            responser.publish(&str_msg);
            
        }
    }
}

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