File content as of revision 2:cec0c1ff1c30:

/*
 * rosserial Publisher for encoder
 * Publish: distance(cm)
 * Subscribe: --once
 */

#include"mbed.h"
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Empty.h>
#include "QEI.h"

using namespace std_msgs;
using namespace ros;

DigitalOut myled(LED1);

#define BAUD 115200

// GYO
Serial gy25(PA_9, PA_10, 115200);
float g[3];
char rxC[8];
char ctBuffer[128];

void rxIRQ(){
    // get the Byte0: 0xAA Preamble Flags
    rxC[0] = gy25.getc();
    // fetch the 8-byte data when 0xAA is found
    if (rxC[0] == 0xAA) {
        for (int i = 1; i < 8; i++) {
            rxC[i] = gy25.getc();
        }
        // if the last byte is 0x55(Frame end flag), starts the calculation
        if (rxC[7] == 0x55) {
            for (int i = 0, j =1; i < 3; i++, j+=2) {
                //Angle = ( (HIGH << 8) | LOW ) / 100
                g[i] = (float)(rxC[j]<<8 | rxC[j+1])/100;
                // adjustment
                if (g[i] > 475)
                    g[i] = g[i] - 476 + 180;
            }
        }
    }
}

// ENCODER
#define N 1000
#define C 10     // circumference(cm)
QEI En_X1(PC_10, PC_11, PC_12, N, QEI::X4_ENCODING); // A, B, Z, pulses/revolution, mode
QEI En_X2(PC_1, PC_2, PC_3, N, QEI::X4_ENCODING);
QEI En_Y1(PB_3, PB_4, PB_5, N, QEI::X4_ENCODING);
QEI En_Y2(PA_6, PA_7, PA_8, N, QEI::X4_ENCODING);

// instantiate the node handle
ros::NodeHandle  nh;

// instantiate the publisher
Float32 X1, X2, Y1, Y2, G1, G2, G3;
Publisher pub_X1("X1", &X1);
Publisher pub_X2("X2", &X2);
Publisher pub_Y1("Y1", &Y1);
Publisher pub_Y2("Y2", &Y2);
Publisher pub_G1("G1", &G1);
Publisher pub_G2("G2", &G2);
Publisher pub_G3("G3", &G3);

// BUTTON for reset
InterruptIn button(USER_BUTTON);

void pressed() {
    gy25.putc(0xA5);
    gy25.putc(0x55);
    gy25.putc(0xA5);
    gy25.putc(0x52);
    En_X1.reset();
    En_X2.reset();
    En_Y1.reset();
    En_Y2.reset();
}

// for reset
void messageCb(const Empty& toggle_msg){
    pressed();
    myled = !myled;   // blink the led
}

Subscriber<Empty> sub("toggle_led", &messageCb);

// initializing node
void nodeInit() {
    nh.getHardware()->setBaud(BAUD);
    nh.initNode();
    
    nh.subscribe(sub);
    
    nh.advertise(pub_X1);
    nh.advertise(pub_X2);
    nh.advertise(pub_Y1);
    nh.advertise(pub_Y2);
    nh.advertise(pub_G1);
    nh.advertise(pub_G3);
    nh.advertise(pub_G2);
}

float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0;

int main() {
    nodeInit();
    
    // gyo
    gy25.format(8,SerialBase::None,1);
    gy25.attach(rxIRQ, Serial::RxIrq);
    // reset button
    button.fall(&pressed);
    
    while (1) {
        X1.data = (float)En_X1.getPulses()/(4*N) * C;
        Y1.data = (float)En_Y1.getPulses()/(4*N) * C;
        X2.data = (float)En_X2.getPulses()/(4*N) * C;
        Y2.data = (float)En_Y2.getPulses()/(4*N) * C;
        G1.data = g[0];
        G2.data = g[1];
        G3.data = g[2];
        if(pX1 != X1.data){
            pX1 = X1.data;
            pub_X1.publish( &X1 );
        }
        if(pX2 != X2.data){
            pX2 = X2.data;
            pub_X2.publish( &X2 );
        }
        if(pY1 != Y1.data){
            pY1 = Y1.data;
            pub_Y1.publish( &Y1 );
        }
        if(pY2 != Y2.data){
            pY2 = Y2.data;
            pub_Y2.publish( &Y2 );
        }
        if(pG1 != G1.data){
            pG1 = G1.data;
            pub_G1.publish( &G1 );
        }
        if(pG2 != G2.data){
            pG2 = G2.data;
            pub_G2.publish( &G2 );
        }
        if(pG3 != G3.data){
            pG3 = G3.data;
            pub_G3.publish( &G3 );
        }
        nh.spinOnce();
        wait_ms(50); // 20hz
    }
}