albatross
/
XBusServoTest
XBusServoの設定を行うプログラムです。
Diff: main.cpp
- Revision:
- 0:301100d3469a
- Child:
- 1:fee82ed64991
- Child:
- 2:32aba462e4df
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jan 04 09:10:05 2017 +0000 @@ -0,0 +1,126 @@ +#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); + +// +// 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; + +string Deci2Hex(int val,bool lower = false){ + string str = "0x"; + char answer[10]; + char henkan[17] = "0123456789ABCDEF"; + int i = 0,j = 0; + while(val > 0){ + j = val % 16; + val = val / 16; + answer[i] = henkan[j]; + i++; + } + i--; + for(j = i;j >= 0;j--){ + str += answer[j]; + } + return str; +} + +//============================================================= +// XbusIntervalHandler() +// play motion ! +//============================================================= +void XbusIntervalHandler() +{ + uint16_t value; + uint16_t diff = kMotionEndMark - kMotionMinMark; + // send current data + gXBus.sendChannelDataPacket(); + value = (uint16_t)(diff * analog.read()) + kMotionMinMark; + gXBus.setServo(servoChannel, value); +} + +//============================================================= +// 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() +{ + if (init() != kXBusError_NoError) + return -1; + + // start motion + gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); + + while(1) { + } +}