albatross
/
XBusServoTest
XBusServoの設定を行うプログラムです。
main.cpp
- Committer:
- YusukeWakuta
- Date:
- 2017-01-11
- Revision:
- 3:db5a2f683282
- Parent:
- 2:32aba462e4df
- Child:
- 4:bde6d41517d3
File content as of revision 3:db5a2f683282:
#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); wait(0.1); } }