File content as of revision 6:46e625708076:

#include "mbed.h"
#include "Servo.h"

#include "LSCServo.h"
#include "DC_Motor_Controller.h"

#include <ros.h>
#include "std_msgs/Bool.h"
#include "geometry_msgs/Twist.h"

#define BAUD_RATE 115200

using namespace std;
using namespace std_msgs;
using namespace ros;
using namespace geometry_msgs;

DC_Motor_PID motor(D6,D3,D4,D5,792); //out1 out2 in1 in2 ppr

DigitalOut myled = LED1;
DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
DigitalOut pin_A = D12;         //not in use
DigitalIn but(D9);              // firing pin
DigitalOut up = D11; //pneumatic part
DigitalOut down = D10; //pneumatic part

LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx

NodeHandle nh;      //ROS node


void messagesqu(const Bool& _msg){  //callback of square
    bool check = _msg.data;
    if(check)
        robot_arm.LobotSerialActionGroupRun(0,1);
}

void messagecro(const Bool& _msg){  //callback of cross
    bool check = _msg.data;
    if(check)
        robot_arm.LobotSerialActionGroupRun(1,1);
}


void messagecir(const Bool& _msg){  //callback of circle
    bool check = _msg.data;
    if(check)
        robot_arm.LobotSerialActionGroupRun(2,1);
}

void messagetri(const Bool& _msg){  //callback of triangle
    bool check = _msg.data;
    if(check)
        robot_arm.LobotSerialActionGroupRun(3,1);
}

void messageL1(const Bool& _msg){   //callback of L1  up and turn
    bool check = _msg.data;
    if(check){
        up = 1;
        wait(0.1);
        robot_arm.LobotSerialActionGroupRun(3,1);
        up = 0;
        motor.move_angle(412);
        }
}

void messageR1(const Bool& _msg){   //callback of R1, not in use for now   up and turn
    bool check = _msg.data;
    if(check){
        up = 1;
        wait(0.1);
        robot_arm.LobotSerialActionGroupRun(3,1);
        up = 0;
        motor.move_angle(412);
        }
}

void messageL3(const Bool& _msg){   //callback of R3, not in use for now   turn 10 cm
    bool check = _msg.data;
    if(check){
        motor.move_angle(412);
        }
}

void messageR3(const Bool& _msg){   //callback of R3, not in use for now   turn 10 cm
    bool check = _msg.data;
    if(check){
        motor.move_angle(100);
        }
}

void messageps(const Bool& _msg){   //callback of ps, not in use for now   turn back to 0 pulse
    bool check = _msg.data;
    if(check){
        motor.set_out(0.4,0);
        while(1)
        {
            if(but)
            {
                motor.set_out(0,0);
                break;
            }
        }
        motor.reset();
        wait(0.5);
        motor.move_angle(615);
        }
}

void messagepad(const Bool& _msg){   //callback of ps, not in use for now   arm go down and release
    bool check = _msg.data;
    if(check){
//        robot_arm.LobotSerialActionGroupRun(3,1);
//        down = 1;
//        wait(0.1);
//        down = 0;
        motor.goto_angle(0);
        }
}

void messagekeypad (const Twist& _msg){     //callback of keypad
    float x = _msg.linear.x;
    float y = _msg.linear.y;
    if (x==1){                                 //  Rotate motor clockwise
        motor.set_out(1, 0);
    }
    else if(x==-1) {                          //  Rotate motor anti-clockwrise 
        motor.set_out(0, 1);
    }
    else{
        motor.set_out(0, 0);
    }
    if (y==1){                                  //  pump air of pnumetic part
        up = 1;
    }
    else if(y==-1) {                          //  release air of pnumetic part
        down = 1;
    }
    else{
        up = 0;
        down = 0;
    }
}

Subscriber<Bool> subsqu("button_square", &messagesqu);
Subscriber<Bool> subcro("button_cross", &messagecro);
Subscriber<Bool> subcir("button_circle", &messagecir);
Subscriber<Bool> subtri("button_triangle", &messagetri);
Subscriber<Bool> subL1("button_L1", &messageL1);
Subscriber<Bool> subR1("button_R1", &messageR1);
Subscriber<Bool> subL3("button_L3", &messageL3);
Subscriber<Bool> subR3("button_R3", &messageR3);
Subscriber<Bool> subps("button_ps", &messageps);
Subscriber<Bool> subpad("button_pad", &messagepad);
Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);

int main() {
    myled = 0;
    but.mode(PullDown);
    
    motor.set_pid(0.008, 0, 0, 0.10);
    
    nh.getHardware()->setBaud(BAUD_RATE);   //set baud rate
    nh.initNode();
    
    nh.subscribe(subsqu);
    nh.subscribe(subcro);
    nh.subscribe(subcir);
    nh.subscribe(subtri);
    nh.subscribe(subL1);
    nh.subscribe(subR1);
    nh.subscribe(subL3);
    nh.subscribe(subR3);
    nh.subscribe(subkeypad);
    nh.subscribe(subps);
    nh.subscribe(subpad);

    while(true) {
        nh.spinOnce();
        if(nh.connected())
            myled = 1;
        else
            myled = 0;
    }
}