File content as of revision 2:611a5eb132a1:

#include "mbed.h"
#include "millis.h"

/*
 * ActionEncoder.cpp
 *
 *  Created on: 7 Mar 2018
 *      Author: tung
 */

#include "ActionEncoder.hpp"
#include "Timer.h"

Serial Action(D8,D2 ); // tx, rx
Serial pc(USBTX, USBRX);



union {
    uint8_t data[24];
    float val[6];
} posture;
int count=0;
int i=0;
int done=0;
float xangle=0;
float yangle=0;
float zangle=0;
float d_angle=0;
float pos_x=0;
float pos_y=0;
float angleSpeed=0;
float temp_zangle=0;
int   LastRead=0;
bool newDataArrived=false;

char buffer[8];


void ActionEncoder_init()
{
    count=0;
    i=0;
    done=0;
    xangle=0;
    yangle=0;
    zangle=0;
    d_angle=0;
    pos_x=0;
    pos_y=0;
    angleSpeed=0;
    temp_zangle=0;
    LastRead=0;
    newDataArrived=false;


}

void readEncoder(char c)
{


    //sprintf(buffer,"%02X",c);
    //sprintf(buffer,"%X",c);
    //pc.printf(buffer);
    //pc.printf("\r\n");
    
    //sprintf(buffer,"%d",count);
    //pc.printf(buffer);
    //pc.printf("\r\n");
    switch(count) {
        case 0:
            if (c==0x0d) count++;
            //else count=0;
            break;
        case 1:
            if(c==0x0a) {
                i=0;
                count++;
            } else if(c==0x0d) {}
            //else count=0;
            break;
        case 2:
            posture.data[i]=c;
            i++;
            if(i>=24) {
                i=0;
                count++;
            }
            break;
        case 3:
            if(c==0x0a)count++;
            //else count=0;
            break;
        case 4:
            if(c==0x0d) {
                d_angle=posture.val[0]-temp_zangle;
                if (d_angle<-180) {
                    d_angle=d_angle+360;
                } else if (d_angle>180) {
                    d_angle=d_angle-360;
                }
                zangle+=d_angle;
                temp_zangle=posture.val[0];
                xangle=posture.val[1];
                yangle=posture.val[2];
                pos_x=posture.val[3];
                pos_y=posture.val[4];
                angleSpeed=posture.val[5];
                newDataArrived=true;
                
                pc.printf("GET\r\n");
                sprintf(buffer, "%f", pos_x);
                pc.printf(buffer);
                pc.printf("  ");
                sprintf(buffer, "%f", pos_y);
                pc.printf(buffer);
                pc.printf("  ");
                sprintf(buffer, "%f", zangle);
                pc.printf(buffer);
                pc.printf("\r\n");
            }
            count=0;
            done=1;
            LastRead=millis();
            break;
        default:
            //count=0;
            break;
    }
}

bool updated()
{
    if (done) {
        done=false;
        return true;
    } else {
        return false;
    }

}

float getXangle()
{
    return xangle;
}

float getYangle()
{
    return yangle;
}

float getZangle()
{
    return zangle;
}

float getXpos()
{
    return pos_x;
}

float getYpos()
{
    return pos_y;
}

float getAngleSpeed()
{
    return angleSpeed;
}

bool isAlive()
{
    if ((millis()-LastRead)<100) {
        return true;
    } else {
        return false;
    }
}

bool newDataAvailable()
{
    if (newDataArrived) {
        newDataArrived=false;
        return true;
    } else return false;
}

char* reset()
{
    return "ACT0";
}

char* calibrate()
{
    return "ACTR";
}
//////////////

void action_get()
{

    /*

    // Loop just in case more than one character is in UART's receive FIFO buffer
    // Stop if buffer full
      while ((device.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
          rx_buffer[rx_in] = device.getc();
    // Uncomment to Echo to USB serial to watch data flow
    //        monitor_device.putc(rx_buffer[rx_in]);
          rx_in = (rx_in + 1) % buffer_size;
      }
      */

    readEncoder(Action.getc());

}
/////////////////////////





int main()
{
    //Action.attach(&action_get, Serial::RxIrq);
    Action.baud(115200);
    Action.format(8,SerialBase::None,1); 
    
    
    ActionEncoder_init();

    while(1) {

        while (Action.readable()==1 ) 
        {
            //pc.printf("readable\r\n");
            char c = Action.getc();
            
            readEncoder(c);
            //pc.putc(c);
            //pc.printf("\r\n");
          
          
          /*
                sprintf(buffer, "%f", pos_x);
                pc.printf(buffer);
                sprintf(buffer, "%f", pos_y);
                pc.printf(buffer);
                sprintf(buffer, "%f", zangle);
                pc.printf(buffer);
                pc.printf("\r\n");*/
            

        }

    }



}