File content as of revision 8:9abe58bd7c07:

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

// 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"


// 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("PotCoordinate", msgCam);
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);
// Faulhaber1 pins
DigitalOut pin1 = A3;  // pin 1 of faulhaber1
DigitalOut pin2 = A5; // pin 2 of faulhaber1
DigitalOut pin3 = A4; // pin 3 of faulhaber1
DigitalOut pin4 = A0; // pin 4 of faulhaber1
AnalogOut aout = D13;
// Faulhaber2 pins
DigitalOut faul2_1 = A1; // pin1 of faulhaber2
DigitalOut faul2_2 = A2; // pin2 of faulhaber2
// Electromagnet
DigitalOut lock = D14; 
// Firing pin
DigitalIn firing1 = D2; // Reset Faulhaber
DigitalIn firing2 = D15; // Load 1 arrow
DigitalIn firing3 = D7; // Pick, short
DigitalIn firing4 = D4; // Pick, longError: Identifier "faul2_1" is undefined in "main.cpp", Line: 241, Col: 14
DigitalIn firing5 = D8; // Pick, long - 2
DigitalIn firing6 = D9;
// Valves
DigitalOut valve = D3; // valve 

// 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, D12, 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; 
int auto_shooting_goToTargetPos = 0;
int start = 0; // control variable for resetting shooter
int touch = 0;
// cam callback
void msgCam(const std_msgs::Int16 &cam_msg)
{
    if( cam_msg.data > 0 && auto_shooting)
    {
        
        int outValue[3] = {cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10};
        if(auto_shooting && auto_shooting_goToTargetPos)
        {
            pin1 = 0;
            pin2 = 0;
            pin3 = 1;
            wait(0.2);
            aout = 1;
            pin3 = 1;
            pin2 = 1;
            pin1 = 1;
            wait(0.1);
            auto_shooting_goToTargetPos = false;
            
            for(int i = 0; i<3; i++){
                //int val[4] = {outValue[i]/8, outValue[i]/4%2, outValue[i]/2%2, outValue[i]%2};
                aout = outValue[i]/8;
                pin3 = outValue[i]/4%2;
                pin2= outValue[i]/2%2;
                pin1 = outValue[i]%2;
                wait(0.1);
                aout = 1;
                pin3 = 1;
                pin2 = 1;
                pin1 = 1;
                wait(0.1);
            }
            aout = 0; pin3 = 0; pin2 = 0; pin1 = 0; pin4 = 0;
        }
        memset(msg2, 0, sizeof(msg2));
        strcpy(msg2, "Cam distance: ");
        itoa(cam_msg.data, msg2 + strlen(msg2), 10);
        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:
        valve = 1 - valve;
        strcpy(msg + len, "x is pressed, valve is ");
        msg[strlen(msg)] = '0' + int(valve);
        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;
        auto_shooting_goToTargetPos = true;
        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;
        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:
       strcpy(msg + len, "right arrow is pressed. ");
        msg[strlen(msg)] = '0' + n_loaded;
        dc_motor1.set_out(0,1);    
        n_loaded++;  
        while(firing2 == 0)
        {
            wait(0.05);
        }

        if(n_loaded <= 5)
        {
            dc_motor1.set_out(0,0.7);
            wait(1.4);     
            dc_motor1.set_out(0,0);           
        }
        else
        {
            wait(0.21);
            dc_motor1.set_out(0,0);  
            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;
        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, phase is ");
        msg[strlen(msg)] = '0' + stage_pick;
        strcpy(msg + strlen(msg), " ");
        
        if(stage_pick == 0)
        {
            strcpy(msg + strlen(msg), "dc_motor reset  ");
            dc_motor2.set_out(0.5, 0);
            while(firing3 == 0);
            if(firing3 == 1)
            {
                dc_motor2.set_out(0,0);
                strcpy(msg + strlen(msg), "short: firing3 pin detected  ");
            }
            stage_pick++;
//            eject_1 = 1; eject_2 = 0;
        }
        else if(stage_pick == 1)
        {
            strcpy(msg + strlen(msg), "faulhaber 2 RefPos ");
            faul2_1 = 1;
            while(firing4 == 0);
            faul2_1 = 0;  
            strcpy(msg + strlen(msg), "long: firing4 detected "); //內
            stage_pick++;      
            
        }
        else if(stage_pick == 2)
        {
            strcpy(msg + strlen(msg), "dc_motor rotate to pick up ");
            dc_motor2.set_out(0,0.5);
            wait(0.2);
            dc_motor2.set_out(0,0);
            stage_pick++;
//            stage_pick = 0;
            
        }
        else if(stage_pick == 3)
        {
            strcpy(msg + strlen(msg), "faul2 to pick the arrows "); // 外
            faul2_2 = 1;
            while(firing5 == 0);
            wait(0.3);
            faul2_2 = 0;
            strcpy(msg + strlen(msg), "Firing5 touched "); 
            
            stage_pick++; // use firing 5
//            stage_pick = 0;
        }
        else if(stage_pick == 4)
        {                
            strcpy(msg + strlen(msg), "faul2 rotate to vertical \n\r"); // 內一啲
            faul2_1 = 1;
            wait(2.6);
            faul2_1 = 0;
            stage_pick++;
//            stage_pick = 0;
        }
        else if(stage_pick == 5)
        {
            strcpy(msg + strlen(msg), "dc_motor rotate to final pos\n\r");
            dc_motor2.set_out(0,1);
//            wait(2); // when there is load; otherwise 0.3
            while(firing6 == 0);
//            {
//                wait(0.025);
//            }
            dc_motor2.set_out(0,0);
            stage_pick++;
//            stage_pick = 0;
        }
        else if(stage_pick == 6)
        {
            strcpy(msg + strlen(msg), "faul2 rotate to final pos \n\r"); // 內一啲
            faul2_1 = 1;
            wait(1.9);
            faul2_1 = 0;
            stage_pick = 0;
        }
        
        break;
    case -8:
    
        break;
    case 9:
        strcpy(msg + len, "L1 is pressed");
        
        break;
    case -9:
//        pin3 = 0; pin4 = 0;
        strcpy(msg + len, "L1 is released");
        break;
    case 10:
        
        dc_motor1.set_out(0,0);
        dc_motor2.set_out(0,0);
        auto_shooting = 1;
        pin4 = 1; 
        n_loaded = 6;
        strcpy(msg + len, "R1 is pressed, auto_shooting is ");
        msg[strlen(msg)] = '0' + auto_shooting;
        break;
    case -10:
        wait(0.1);
        auto_shooting = 0;
        pin4 = 0; 
        strcpy(msg + len, "R1 is released");
        break;
    case 11:
        strcpy(msg + len, "share is pressed");  
        strcpy(msg + strlen(msg), "Start resetting Faulhaber: firing1 is ");
        msg[strlen(msg)] = '0' + firing1.read();
        responser.publish(&str_msg);
        if(!faul_reset && firing1 == 0)
        {
            pin2 = 0; pin1 = 1; pin3 = 1; // 101
            memset(msg, 0, sizeof(msg));
            
//                faul_reset = 1;
        }
         while(firing1 == 0);
        pin1 = 0; pin2 = 1; pin3 = 1; // 110
        wait(0.3);
        pin1 = 0; pin2 = 0; pin3 = 0;
//        memset(msg, 0, sizeof(msg));
        strcpy(msg + strlen(msg), " case 11 firing1 is touched");
              
        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;  
    valve = 0; faul2_1 = 0; faul2_2 = 0;
    
    // Initiate firing pins
    firing1.mode(PullUp); // default 0
    firing2.mode(PullUp); // default 0
    firing3.mode(PullUp); // default 0
    firing4.mode(PullUp); // default 0
    firing5.mode(PullUp); // default 0
    firing6.mode(PullUp); // default 0
    // 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;
//    aout.period_us(0.01); 
    aout = 0;
    // 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

    
    }
}

 
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, "R2: the value is ");
        len = strlen(msg);
        itoa(joystick_msg.data[2], msg + len, 10);
        
    }

    responser.publish(&str_msg);
}