File content as of revision 0:62729afe5606:

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

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

// 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
std_msgs::Float32 X1, X2, Y1, Y2, G1, G2, G3;
ros::Publisher pub_X1("X1", &X1);
ros::Publisher pub_X2("X2", &X2);
ros::Publisher pub_Y1("Y1", &Y1);
ros::Publisher pub_Y2("Y2", &Y2);
ros::Publisher pub_G1("G1", &G1);
ros::Publisher pub_G2("G2", &G2);
ros::Publisher pub_G3("G3", &G3);

// initializing node
void nodeInit() {
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    
    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);
}

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

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];
        pub_X1.publish( &X1 );
        pub_Y1.publish( &Y1 );
        pub_X2.publish( &X2 );
        pub_Y2.publish( &Y2 );
        pub_G1.publish( &G1 );
        pub_G2.publish (&G2 );
        pub_G3.publish( &G3 );
        nh.spinOnce();
        wait_ms(50); // 20hz
    }
}