File content as of revision 1:4fb26a2daf03:

#include "mbed.h"
#include "Servo.h"
union floatInByte
{
    unsigned short         sd;
    unsigned char c[2];
};

Serial master(dp16,dp15);
Servo arm[4] = {Servo(dp1),Servo(dp2),Servo(dp24),Servo(dp18)};
floatInByte data[4][2] = {{0,0},{0,0},{0,0},{0,0}};

char RXDATA[24] = {0,0,0,0,255,255,155,255,5,255,000,255,
                   0,0,0,0,255,255,155,225,5,205,000,55};

void run(int i);
void rx(){
    if(master.getc() == 'H'){
        for(int i = 1;i < 24;i++){
            RXDATA[i] = master.getc();
        }
    }
}
void waitTime(float ti){
    Timer t;
    t.start();
    while(ti > t.read());
    t.stop();
    return;
}
#if 0//R
#define head 4
    void run(int i){
        //for(int i = 0 ; i < 4 ; i ++ ){
        for(int j = 0 ; j < 2; j++){
            data[i][0].c[j] = RXDATA[head + i*2 + j];   
        }
        arm[i] = (float)data[i][0].sd/0xffff;
        //printf("%d %f  ",i,float(data[i].sd)/0xffff);
        //}
        //printf("\n");
    }
#else//L
#define head 4+12
    void run(int i){
        //for(int i = 0 ; i < 4 ; i ++ ){
        for(int j = 0 ; j < 2; j++){
            data[i][0].c[j] = RXDATA[head + i*2 + j];
        }
        arm[i] = (float)data[i][0].sd/0xffff;
        //printf("%d %f %d %d ",i,float(data[i].sd)/0xffff , data[i].c[1] , data[i].c[0]);
        //}
        //printf("\n");
    }
#endif

double armSampleTime[4] = {0,0,0,0};
bool armFlag[4] = {false,false,false,false};
void update(int i){
        //armSampleTime[i] += 10.0/1000.0;
        double r = (float)data[i][0].sd/0xffff;
        double a = (float)data[i][1].sd/0xffff;
        double c = r + (a-r)*exp(-1.0/(10.0/1000.0)*armSampleTime[i]);
        printf("%5f %5f",r,a);arm[i] = r;
        if(fabs(r-c) < 0.001F)
        {
            data[i][1] = data[i][0];
            armSampleTime[i] = 0;
            armFlag[i] = true;
        }
        printf("%5f ",c);//arm[i] = c;
}
double armOffset[4] = {0,0,0,0};
int main() {
    master.baud(115200);
    master.attach(&rx);
    //printf("start ");
    //pc.baud(115200);
    while(1) {
        //pc.printf("%x ",master.getc());
        //for(int i = 0 ; i<100 ; i++){
        //
        /*
        for(int i = 0 ; i < 4;i++){
            update(i);
            run(i);
            
        }
        waitTime(0.01);
        */
        for(int i  = 0 ;  i < 4 ; i++)run(i);
        waitTime(20.0/1000.0);
    }
}