File content as of revision 3:759af41b06ee:

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

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



#define GET_LOW_char(A) (uint8_t)((A))
//Macro function  get lower 8 bits of A
#define GET_HIGH_char(A) (uint8_t)((A) >> 8)
//Macro function  get higher 8 bits of A
#define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
//put A as higher 8 bits   B as lower 8 bits   which amalgamated into 16 bits integer

#define ID0 0
#define ID1 1
#define ID2 2
#define ID3 3
#define ID4 4

#define LOBOT_SERVO_FRAME_HEADER         0x55
#define LOBOT_SERVO_MOVE_TIME_WRITE      1

#define BAUD_RATE 115200

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

DigitalOut out1(D2);    //  Motor direction control pin 1
DigitalOut out2(D4);    //  Motor direction control pin 2
DigitalIn home_button(D13);     // Button for setting the home button


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;

BufferedSerial serial_port(D1, D0, 16, 16); // tx, rx, buffer size, multiple

NodeHandle nh;      //ROS node

Servo servo = D6;

QEI dc_motor (D8,D7,NC,792);    // Create QEI object for increment encoder
int counter = 0;    // Dummy counter for sample program

void reset(){       // Setting home point for increment encoder for stepper motor
    while (home_button == 0){       // Continue to turn clockwise until home button pressed
        out1 = 1;
        out2 = 0;
    }
    out1 = 0;
    dc_motor.reset();       //  Reset pulse_
    wait(1);
 };
 
 void go_angle(int angle){      //  Move motor to specific angle from home point
    int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0);    //  Calculating target pulse number
    int cur_pulse = dc_motor.getPulses();
    
    while ( tar_pulse != cur_pulse ){

        if (tar_pulse > cur_pulse){     //  Rotate motor clockwise
            out1 = 1;
            out2 = 0;
        }
        else {                          //  Rotate motor counter clockwrise 
            out1 = 0;
            out2 = 1;
        }
        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
    }
    out1 = 0;       //  Stop motor
    out2 = 0;
};

void LobotSerialoneServoMove(uint8_t id, int16_t position, uint16_t time)//robot arm move dedicated position
{
  char buf[10];
  if(position < 0)
    position = 0;
  if(position > 1000)
    position = 1000;
  buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
  buf[2] = 0x08;
  buf[3] = 0x03;
  buf[4] = 0x01;
  buf[5] = GET_LOW_char(time);
  buf[6] = GET_HIGH_char(time);
  buf[7] = id;
  buf[8] = GET_LOW_char(position);
  buf[9] = GET_HIGH_char(position);
  serial_port.write(buf, 10);
}

void SerialMoveActionGroup(uint8_t gpid, uint16_t times)//robot arm move action group(No. action group, repeat time)
{
    
    char buf[7];
    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
    buf[2]=0x05;
    buf[3]=0x06;
    buf[4]=gpid;
    buf[5]=GET_LOW_char(times);
    buf[6]=GET_HIGH_char(times);
    serial_port.write(buf,7);
    
}
void messagesqu(const Bool& _msg){  //callback of square
    bool check = _msg.data;
    if(check)
        //SerialMoveActionGroup(ID1,1);
        SerialMoveActionGroup(ID0,1);
}

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


void messagecir(const Bool& _msg){  //callback of circle
    bool check = _msg.data;
    if(check)
        //SerialMoveActionGroup(ID1,1);
        SerialMoveActionGroup(ID2,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)
        //SerialMoveActionGroup(ID1,1);
        SerialMoveActionGroup(ID3,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
        out1 = 1;
        out2 = 0;
    }
    else if(x==-1) {                          //  Rotate motor anti-clockwrise 
        out1 = 0;
        out2 = 1;
    }
    else{
        out1 = 0;
        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;
    }
}