File content as of revision 5:03af248f3f7c:

// 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/Int16.h>
#include <std_msgs/String.h>
#include <std_msgs/Int16MultiArray.h>
#include "DC_Motor_Controller.h"
//#include "DC_Motor.h"

// Declare Functions
void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
void msgCb_button(const std_msgs::Int8 &button_msg);
void msgCam(const std_msgs::Int16 &cam_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);
ros::Subscriber<std_msgs::Int16> sub_cam("PotDistance", msgCam);
// Pubs
std_msgs::String str_msg;
std_msgs::String str_msg2;
ros::Publisher responser("display", &str_msg);
ros::Publisher responser2("show_cam", &str_msg2);

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

// External Button
DigitalIn button(USER_BUTTON);
// LED
DigitalOut myled(LED1);
// Faulhaber pins
DigitalOut pin1 = A3;  // pin 1 of faulhaber
DigitalOut pin2 = A5; // pin 2 of faulhaber
DigitalOut pin3 = A4; // pin 3 of faulhaber
DigitalOut pin4 = A0; // pin 4 of faulhaber
AnalogOut aout = D13;
// Electromagnet
DigitalOut lock = D14; 
// Firing pin
DigitalIn firing1 = D9; // Reset Faulhaber
DigitalIn firing2 = D15; // Load 1 arrow
DigitalIn firing3 = D2; // Pick, small
// Valves
DigitalOut eject_1 = D4; // valve 1
DigitalOut eject_2 = D7;  // valve 2
// DC motors
DC_Motor_PID dc_motor1(D6, D5, PE_6, PF_8, 792); //M1, M2, INA, INB, PPR; load 1 arrow
DC_Motor_PID dc_motor2(D10, D11, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works?  original: D4, D7 for encoder, D12, D13 for out

// Global Control variable
char msg[200] = {0}; // msg for responser
char msg2[200] = {0}; // msg for cam show
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
int auto_shooting = 0;
int smooth = 0; 

// cam callback
void msgCam(const std_msgs::Int16 &cam_msg)
{
    int minimum = 250;
    int maximum = 350;
//    int data = cam_msg.data;
    if(auto_shooting > 0)
    {
        aout = (cam_msg.data - minimum) / (maximum - minimum);
        memset(msg2, 0, sizeof(msg2));
        strcpy(msg2, "Cam distance: ");
        itoa(cam_msg.data, msg2 + strlen(msg2), 10);
        strcpy(msg2 + strlen(msg2), ", aout(*1000): ");
        itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10);
//        sprintf("Cam distance: %d, %.4f", data, aout.read());
        responser2.publish(&str_msg2);
    }
}

// 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);
        // ---------- added for clearing n_loaded -------------
        n_loaded = 6;         
        strcpy(msg + strlen(msg), "n_loaded is ");
        msg[strlen(msg)] = '0' + n_loaded;
        // ---------- end --------------
        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:
        pin3  = 1;
        strcpy(msg + len, "triangle is pressed, pin3 is ");
        msg[strlen(msg)] = '0' + int(pin3);
        break;
    case -3:
        pin3 = 0; 
        strcpy(msg + len, "triangle is released, pin3 is ");
        msg[strlen(msg)] = '0' + int(pin3);
        break;
    case 4:
        strcpy(msg + len, "square is pressed");
        pin1 = 1; pin2 = 1; pin3 = 0; lock = 1;
        break;
    case -4:
        strcpy(msg + len, "square is released");
        pin1 = 0; pin2 = 0;
        break;
    case 5:
        
        pin1 = 0;
        pin3 = 0;
        if(smooth)
            pin4 = 1;
        pin2 = 1;
        strcpy(msg + len, "down arrow is pressed, pin2 is ");
        msg[strlen(msg)] = '0' + int(pin2);
        break;
    case -5:
        pin2 = 0; pin4 = 0;
        strcpy(msg + len, "down arrow is released, pin2 is ");
        msg[strlen(msg)] = '0' + int(pin2);
        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++;  
        while(firing2 == 0);

        if(n_loaded <= 5)
        {
            dc_motor1.set_out(0,0.5);
            wait(1.7);     
            dc_motor1.set_out(0,0);           
        }
        else
        {
            load_manual = 1;
            strcpy(msg + len, "right arrow is pressed. ");
            strcpy(msg + strlen(msg), "n_loaded cleared!");
            n_loaded = 0;            
        }
            
        break;
    case -6:
        
        strcpy(msg + len, "right arrow is released. ");
        break;
    case 7:
        
        pin2 = 0;
        if(smooth)
            pin4 = 1;
        pin1 = 1;
        strcpy(msg + len, "up arrow is pressed, pin1 is");
        msg[strlen(msg)] = '0' + int(pin1);
        break;
    case -7:
        pin1 = 0;
        strcpy(msg + len, "up arrow is released, pin1 is");
        msg[strlen(msg)] = '0' + int(pin1);
        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:
        auto_shooting = 1 - auto_shooting;
        if(auto_shooting)
        {
            pin4 = 1; pin3 = 1; pin1 = 0; pin2 = 0;
        }
        else
        {
            pin3 = 0; pin4 = 0; // dun change order, in case delay           
        }
        
        strcpy(msg + len, "L1 is pressed, auto_shooting is ");
        msg[strlen(msg)] = '0' + auto_shooting;
        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:
        smooth = 1 - smooth;
        strcpy(msg + len, "share is pressed, smooth: ");
        msg[strlen(msg)] = '0' + smooth;
        
        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:
        pin1 = 1; pin2 = 1; pin3 = 1;
        strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
        break;
    case -13:
        pin1 = 0; pin2 = 0; pin3= 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);
    nh.advertise(responser2);
    // Initiate display
//    msg = "------- LAST DAY -------";
    str_msg.data = msg;
    str_msg2.data = msg2;
//    responser.publish(&str_msg);
//    responser2.publish(&str_msg2);
    // Initiate subscribers
    nh.subscribe(sub_button);
    nh.subscribe(sub_joystick);
    nh.subscribe(sub_cam);
    // Initiate IOs, keep everything at rest
    myled = 0;
    pin1 = 0;pin2 = 0; pin3 = 0; lock = 0;  
    eject_1 = 0; eject_2 = 1;
    
    // Initiate firing pins
    firing1.mode(PullUp); // default 0
    firing2.mode(PullUp); // default 1
    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);
//    up = 1; pin2 = 1; pin2 = 0;
    // 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 && firing1 == 0)
//            if(1)
            {
                pin2 = 0; pin1 = 1; pin2 = 1; // 101
                start = 1;
                memset(msg, 0, sizeof(msg));
                strcpy(msg, "Start resetting Faulhaber: firing1 is ");
                msg[strlen(msg)] = '0' + firing1.read();
                responser.publish(&str_msg);
//                faul_reset = 1;
            }
            else
            {
                dc_motor1.set_out(0,1); 
                load_manual = 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;           
            strcpy(msg + strlen(msg), ", firing2 is ");
            msg[strlen(msg)] = '0' + firing2.read();
            strcpy(msg + strlen(msg), ", load_manual is ");
            msg[strlen(msg)] = '0' + load_manual;
            responser.publish(&str_msg);
        }
        if(load_manual && firing2 == 1)
        {
            wait(0.17);
            dc_motor1.set_out(0,0);  
            memset(msg, 0, sizeof(msg));
            strcpy(msg, "firing2 is touched: ");
            msg[strlen(msg)] = '0' + firing2.read();             
        }
        if(firing1.read() == 1 && start == 1)
        {
            pin1 = 0; pin2 = 1; pin3 = 1; // 110
            wait(0.3);
            memset(msg, 0, sizeof(msg));
            strcpy(msg, "firing1 is touched: ");
            msg[strlen(msg)] = '0' + firing1.read();
            responser.publish(&str_msg);
            pin3 = 0; pin2 = 0; pin1 = 0;
            start = 0;
            
        }
    }
}

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