質問:https://detail.chiebukuro.yahoo.co.jp/qa/question_detail/q10220411930

#include "mbed.h"
#include "MoterControl.h"
#define CLEAR(x) sprintf(x,"");
#define PID MoterControl::PID

class Operation{
    public:
        int s3axis(double xy_axis,double z_axis){
            int stage_rpm = 0;
            if( 0 < z_axis && z_axis < 0.48){
                stage_rpm = (0.35 - xy_axis)*100;
            }
            return stage_rpm*34;
        }
};
//モーター用定数
const int max_moter = 500;
const int rpm_scan = 50;
const int palse = 16*19;
const double P_gain = 1.00;
const double I_gain = 0.8;
const double D_gain = 0.09;
MoterControl moter1(
    /*モータ出力1のピン名*/ D9,
    /*モータ出力2のピン名*/ D5,
    /*エンコーダ入力1のピン名*/ D7,
    /*エンコーダ入力2のピン名*/ D4,
    /*目標値の上限値(0=デフォルト値:500)*/ max_moter,
    /*RPM値の更新周期(0=デフォルト値:50)*/ rpm_scan,
    /*パルス回転比(パルス/回転)*/ palse, 
    /*デューティー比の演算方法(PID,DOO)*/ PID,
    /*PID:Pゲイン,DOO:乗算ゲイン*/ P_gain,
    /*PID:Iゲイン,DOO:減算ゲイン*/ I_gain,
    /*PID:Dゲイン,DOO:最低補正絶対値*/ D_gain
);
Operation op;
Serial xbee(A0,A1);
InterruptIn SSBT(USER_BUTTON);//ボタン割り込み
//ISコマンド
const char IS[19]= {0x7E,0x00,0x0F,0x17,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFE,0x02,0x49,0x53,0x4E};
//グローバル変数
int c=0,d=0,BT=0,cnt=0,RPM = 0,size = 0;
double x=0,y=0,z=0;
float oldT = 0,newT = 0,sum = 0,vtimer[100],avg = 0;
bool flag = false;
char val[10]="";char val1g[10]="";char val2g[10]="";char val3g[10]="";char val300[300]="";
//ボタン割り込み
void BTH(){
    printf("z = %f[mV],y = %f[mV],x = %f[mV], RPM = %d\r\n",z,y,x,RPM);
}
double vconv(double val){
   double ans;
   ans=1.17*val/1024;
   return ans;
}
//通信兼main
int main(){
    xbee.baud(9600);
    SSBT.rise(&BTH);
    //timer.attach(&submit, 1);
    int a=0,b=0,k=0,timeout = 0;
    SSBT.rise(&BTH);
    while(1){
        //printf("%1.1f,",pw);
        //通信
        if(xbee.readable() == 0 && flag == false){
            for(int i=0;i<19;i++){
                xbee.putc(IS[i]);
            }timeout=0;
            flag = true;
        }else if (xbee.readable() == 1 && flag == true){
            timeout=0;
            size += sprintf(val,"%02x",xbee.getc());
            strcat(val300,val);
        }else if(xbee.readable() == 0 && flag == true){
            timeout++;
            if(timeout > 250000 && size < 58){
                printf("to\n");
                timeout = 0;
                flag = false;
            }else if(timeout < 250000 && size == 58){
                for(a=44;a < (size - 2);){
                    for(b=0;b<4;b++){
                        if(b==0)
                            sprintf(val,"0x%c",val300[a]);
                        else
                            sprintf(val,"%c",val300[a]);
                        if(k==0){strcat(val1g,val);}
                        else if(k==1){strcat(val2g,val);}
                        else if(k==2){strcat(val3g,val);}
                        a++;
                    }k++;
                }k=0;
                z = vconv(atof(val1g));y = vconv(atof(val2g));x = vconv(atof(val3g));
                CLEAR(val1g) CLEAR(val2g) CLEAR(val3g) CLEAR(val300)
                size = 0;
                flag=false;
                printf("z = %f[mV],y = %f[mV],x = %f[mV], RPM = %d\r\n",z,y,x,RPM);
            }//if抜け
        }//if抜け
        //printf("z = %f[mV],y = %f[mV],x = %f[mV], RPM = %d\r\n",z,y,x,RPM);
        //モーター
        RPM = op.s3axis(x,z);
        moter1.setTarget(RPM);
       
    }
}