File content as of revision 3:0de505949008:

/*
 * 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"
#include "math.h"

using namespace std_msgs;
using namespace ros;

DigitalOut myled(LED1);

#define BAUD 115200
#define M_PI 3.14159265358979323846  /* pi */

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


float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0, pXaxis = 0, pYaxis = 0;
float g0 = 0, Xn = 0, Yn = 0, Gn = 0, pX = 0, pY = 0, pg0 = 0;
bool clockwise = false;

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 18     // 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 Xaxis, Yaxis, X1, X2, Y1, Y2, G1, G2, G3;
Publisher pub_X("Xaxis", &Xaxis);
Publisher pub_Y("Yaxis", &Yaxis);
Publisher pub_X1("X1", &X1);
Publisher pub_Y1("Y1", &Y1);
Publisher pub_X2("X2", &X2);
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();
    Xn = 0;
    Yn = 0;
}

// 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_X);
    nh.advertise(pub_Y);
    nh.advertise(pub_X1);
    nh.advertise(pub_Y1);
    nh.advertise(pub_X2);
    nh.advertise(pub_Y2);
    nh.advertise(pub_G1);
    nh.advertise(pub_G2);
    nh.advertise(pub_G3);
}



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_Y2.getPulses()/(4*N) * C;
        X2.data = (float)En_X2.getPulses()/(4*N) * C;
        Y2.data = (float)En_Y1.getPulses()/(4*N) * C;
        
        float dX = ((X1.data - pX1) + (X2.data - pX2)) / 2;
        float dY = ((Y1.data - pY1) + (Y2.data - pY2)) / 2;
        
        if(pG1 < 45 && pG1 >= 0 && floor(g[0]) > 315){
            clockwise = true;
        }
        else if(pG1 > 315 && floor(g[0]) >= 0 && floor(g[0]) < 45){
            clockwise = false; 
        }
        
        if(clockwise){
            g0 = g[0];
            //g0 = g[0] - 360;
        }
        else{
            g0 = g[0];
        }
        
        //float arc = 15 * g0 / 180 * M_PI;
        //float Xa = X1.data - arc;
        //float Ya = Y1.data - arc;
 //       float dX = X - pX;
//        float dY = Y - pY;
        float dg0 = g0 - pg0;
        
        float x = (Xn + dX * cos(Gn + dg0 / 360 * M_PI) + dY * cos(Gn + M_PI/2 + dg0 / 360 * M_PI));
        float y = (Yn + dX * sin(Gn + dg0 /360 * M_PI) + dY * sin(Gn + M_PI/2 + dg0 /360 * M_PI));
        Gn = Gn + dg0 / 180 * M_PI;
        pg0 = g0;
        Xn = x;
        Yn = y;
        //x = (int)(x*10);
        //y = (int)(y*10);
        //Xaxis.data = (float) x;
        //Yaxis.data = (float) y;
        Xaxis.data = (int)x;
        Yaxis.data = (int)y;
//        pX = dX;
//        pY = dY;
        //Xaxis.data = (int) (Xa * cos(g0 / 180 * M_PI) - Ya * sin(g0 / 180 * M_PI));
        //Yaxis.data = (int) (Ya * cos(g0 / 180 * M_PI) + Xa * sin(g0 / 180 * M_PI));
        
        pub_X.publish( &Xaxis );
        pub_Y.publish( &Yaxis );
        
        G1.data = floor(g[0]);
        G2.data = dX;
        G3.data = dY;
        

        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
    }
}