File content as of revision 4:000065db6ae4:

#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_Controller motor(D2,D4,D8,D7,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
DigitalOut pin_B = D11;
DigitalOut pin_C = D10;

LSCServo robot_arm(D1, D0);

NodeHandle nh;      //ROS node

Servo servo = D6;

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);
//if(check && servo == 0) {
//        servo = 1.0;
//    }
//    else if(check && servo == 1)
//    {
//        servo = 0; 
//    }
}

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

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.out1 = 1;
        motor.out2 = 0;
    }
    else if(x==-1) {                          //  Rotate motor anti-clockwrise 
        motor.out1 = 0;
        motor.out2 = 1;
    }
    else{
        motor.out1 = 0;
        motor.out2 = 0;
    }
    if (y==1){                                  //  pump air of pnumetic part
        pin_B = 1;
    }
    else if(y==-1) {                          //  release air of pnumetic part
        pin_C = 1;
    }
    else{
        pin_B = 0;
        pin_C = 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<Twist> subkeypad("button_keypad", &messagekeypad);

int main() {
    myled = 0;
    servo = 0;
    
    nh.getHardware()->setBaud(BAUD_RATE);   //set baud rate
    nh.initNode();
    
    nh.subscribe(subsqu);
    nh.subscribe(subcro);
    nh.subscribe(subcir);
    nh.subscribe(subtri);
    nh.subscribe(subkeypad);
    while(true) {
        nh.spinOnce();
        if(nh.connected())
            myled = 1;
        else
            myled = 0;
    }
}