File content as of revision 1:fee82ed64991:

#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     30      // flame / sec
#define kMotionMinMark      0x1249
#define kMotionEndMark      0xED86

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

//
// global vars
//
unsigned char           gOK2SendPacket = true;
unsigned int            gCurrentPos = kXbusServoNeutral;
long int            gCurrentValue = 0;
unsigned char           gCurrentID = 1;
unsigned char           gCurrentSubID = 0;
uint8_t Angle  = 0x1D;
int16_t AngleValue = 0x01;
static const uint8_t        servoChannel = 0x01;

uint16_t value;
uint16_t diff  = kMotionEndMark - kMotionMinMark;

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

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

        // send current data
        gXBus.sendChannelDataPacket();
    }
}

//=============================================================
// setCurrentValue()
//  set current value to the servo for temp
//=============================================================
void setCurrentValue()
{
    XBusError       result;
    for(;;) {
        result = gXBus.setCommand(Angle, &AngleValue);
        wait_ms(10);
        if (result == kXBusError_NoError)
            break;
    }
}

//=============================================================
// writeCurrentValue()
//  write current value to the servo
//=============================================================
void writeCurrentValue()
{
    XBusError       result;
    for (;;) {
        result = gXBus.setCommand(Angle, &AngleValue);
        wait_ms(10);
        if (result == kXBusError_NoError)
            break;
    }
}
//=============================================================
// 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;
}

int main()
{
    if (init() != kXBusError_NoError)
        return -1;

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

    while(1) {
        gOK2SendPacket = false;
      //  setCurrentValue();
       // writeCurrentValue();

        value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
        gXBus.setServo(servoChannel,value );
        gOK2SendPacket = true;
    }
}