File content as of revision 2:ca608c24a14e:

/* main.cpp file
 *
 * for testing mbed motion player with XBusServo.cpp
 *
 * Copyright (c) 2014-2014 JR PROPO
 * Released under the MIT License: http://mbed.org/license/mit
 *
 * by Zak Sawa
 */

#include "mbed.h"
#include "XBusServo.h"

//#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             PTE0    // PTD3 p13
#define kXBusRx             PTE1    // PTD2 p14
#define kXBusSwitch         D10
#define kMaxServoNum        4       // 1 - 50

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

#define kMotionInterval     30      // flame / sec
#define kMotionEndMark      0xFFFF

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


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


//
// sample motion data
//
static const uint8_t        servoChannel[kMaxServoNum] = {0x01, 0x02, 0x03, 0x04};

static const poseRec        motionData[] = {
//      data type        nextPoseIndex         servoPosition                 frameFromBeforePause
    {dataType_Pose,         1,              {0x7FFF, 0x7FFF, 0x7FFF, 0x7FFF},           10},
    {dataType_Pose,         2,              {0x5FFF, 0x8FFF, 0xAFFF, 0x4FFF},           30},
    {dataType_Pose,         3,              {0x9FFF, 0x5FFF, 0x6FFF, 0xAFFF},           10},
    {dataType_Pose,         4,              {0x9FFF, 0xEFFF, 0x7FFF, 0x1FFF},           20},
    {dataType_Pose,         kMotionEndMark, {0x2FFF, 0x5FFF, 0x8FFF, 0x5FFF},           30}
};


//
// global vars
//
XBusServo                       gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum);
Ticker                          gTimer;
volatile uint16_t               gCurrentFlame = 0;
volatile uint16_t               gCurrentIndex = 0;
volatile uint16_t               gBeforeIndex = 0;
volatile uint16_t               gCurrentServoPos[kMaxServoNum];


//=============================================================
// XbusIntervalHandler()
// play motion !
//=============================================================
void XbusIntervalHandler()
{
    uint16_t            counter;

    // send current data
    gXBus.sendChannelDataPacket();
    if (gCurrentIndex == kMotionEndMark)
        return;

    // make next data
    gCurrentFlame++;

    if (gCurrentFlame == motionData[gCurrentIndex].frameFromBeforePause) {
        gBeforeIndex = gCurrentIndex;
        gCurrentIndex = motionData[gCurrentIndex].nextPoseIndex;
        gCurrentFlame = 0;
    }

    if (gCurrentIndex == kMotionEndMark)
        return;

    if (gCurrentIndex != gBeforeIndex)
        for (counter = 0; counter < kMaxServoNum; counter++) {
            uint16_t            value;
            int32_t             temp;

            value = motionData[gBeforeIndex].servoPosition[counter];
            temp = motionData[gCurrentIndex].servoPosition[counter] - motionData[gBeforeIndex].servoPosition[counter];
            temp *= gCurrentFlame;
            temp /= motionData[gCurrentIndex].frameFromBeforePause;
            value += temp;

            gXBus.setServo(servoChannel[counter], value);
        }
}


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

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

    // initialize XBus Servos
    for (counter = 0; counter < kMaxServoNum; counter++) {
        result = gXBus.addServo(servoChannel[counter], kXbusServoNeutral);
        if (result != kXBusError_NoError) {
            gXBus.stop();
            return result;
        }
    }

    return kXBusError_NoError;
}


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

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

    while(1) {
    }


}