File content as of revision 4:bde6d41517d3:

#include "mbed.h"
#include "XBusServo.h"
#include <string>

//#define DEBUGmain

#ifdef DEBUGmain
#define DBG(fmt) printf(fmt)
#define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
#else
#define DBG(...)
#define DBGF(...)
#endif

//
// defines
//
#define kXBusTx             p9
#define kMaxServoNum        1       // 1 - 50

#define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))

#define kMotionInterval     10      // flame / sec
#define kMotionMinMark      0x1249
#define kMotionEndMark      0xED86

AnalogIn analog(p20);
Serial pc(USBTX,USBRX);
DigitalOut Led(LED1);

//
// typedefs
//
typedef enum poseDataType {
    dataType_Pose,
    numOfDataType
} poseDataType;


typedef struct poseRec {
    poseDataType            poseType;
    uint16_t                nextPoseIndex;
    int32_t                 servoPosition[kMaxServoNum];
    uint16_t                frameFromBeforePause;
}
poseRec;

static const uint8_t        servoChannel = 0x01;

//
// global vars
//
XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
Ticker gTimer;

//=============================================================
// XbusIntervalHandler()
// play motion !
//=============================================================
void XbusIntervalHandler()
{
    uint16_t value;
    uint16_t diff  = kMotionEndMark - kMotionMinMark;
    value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
    gXBus.setServo(servoChannel, value);
    gXBus.sendChannelDataPacket();
}

//=============================================================
// init()
//  initialize all setup
//=============================================================
XBusError init()
{
    XBusError result;

    // initialize XBus
    result = gXBus.start();
    if (result != kXBusError_NoError) {
        gXBus.stop();
        return result;
    }

    // initialize XBus Servos
    result = gXBus.addServo(servoChannel, kXbusServoNeutral);
    if (result != kXBusError_NoError) {
        gXBus.stop();
        return result;
    }

    return kXBusError_NoError;
}

//=============================================================
// main()
//
//=============================================================
int main()
{
    XBusError result;

    if (init() != kXBusError_NoError) {
        return -1;
        Led = 1;
        wait(2);
        Led = 0;
    }

    // start motion
    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);

    while(1) {
        //動作角度を180度幅に拡張
        int16_t value = 0x01;
        int16_t angleParam = 0x0011;
        result = gXBus.setCommand(kXBusOrder_1_Angle_180,&value);
        result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&angleParam);

        //電源が入るとすぐに指示位置に移動
        int16_t quick = 0x00;
        int16_t quickParam = 0x0012;
        result = gXBus.setCommand(kXBusOrder_1_SlowStart,&quick);
        result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&quickParam);

        //信号が途切れたらその位置に固定
        int16_t steer = 0x01;
        int16_t steerParam = 0x0013;
        result = gXBus.setCommand(kXBusOrder_1_StopMode,&steer);
        result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&steerParam);
        
        //ROMから設定を削除する時は、PaParameterResetをsetCOmmandで書き込む

        wait(0.1);

    }
}