Library for XBus servo (under construction)
Dependents: mbed_XBus_Test mbed_XBus_MotionTest XBusServoTest ControlYokutan2017_2 ... more
It's pre-opened page. it's still a little bit unstable to use command packet but mostly work. Tested only on KL25Z
暫定版ページです。 まだコマンドパケット使用時に時々不安定になりますが、概ね動作しています。 KL25Z上でのみ、動作確認しています
Diff: XBusServo.cpp
- Revision:
- 1:bd80d3e8f3a3
- Parent:
- 0:381d475cfd6c
- Child:
- 2:4aca5ffce457
--- a/XBusServo.cpp Thu Oct 02 08:46:53 2014 +0000 +++ b/XBusServo.cpp Wed Oct 08 01:59:16 2014 +0000 @@ -3,11 +3,15 @@ * for mbed * * Copyright (c) 2014-2014 JR PROPO + * Released under the MIT License: http://mbed.org/license/mit + * * by Zak Sawa */ #include "XBusServo.h" #include "pinmap.h" +#include "gpio_api.h" + #define kXBusBaudrate 250000 // bps @@ -49,9 +53,8 @@ #define DEBUG #ifdef DEBUG -Serial pc(USBTX, USBRX); // tx, rx -#define DBG(fmt) pc.printf(fmt) -#define DBGF(fmt, ...) pc.printf(fmt, __VA_ARGS__) +#define DBG(fmt) printf(fmt) +#define DBGF(fmt, ...) printf(fmt, __VA_ARGS__) #else #define DBG(...) #define DBGF(...) @@ -73,19 +76,33 @@ // 2014/09/02 : move from Arduino lib by Sawa //**************************************************************************** XBusServo::XBusServo(PinName tx, PinName rx, uint8_t maxServoNum) - : RawSerial(tx, rx) //, DigitalInOut(tx) + : XBusPort(tx, rx) { - int bufferSize; // channel data packet buffer size - DBG("XBusServo::XBusServo\n"); // initialize serial txPin = tx; txOnly = (rx == NC); + maxServo = maxServoNum; +} + + +//**************************************************************************** +// XBusServo::start +// return : error code +// parameter : none +// +// start to use XBus +// 2014/09/02 : move from Arduino lib by Sawa +//**************************************************************************** +XBusError XBusServo::start() +{ + int bufferSize; // channel data packet buffer size + + DBG("XBusServo::start\n"); // initialise vars numOfServo = 0; - maxServo = maxServoNum; if (maxServo > kXBusMaxServoNum) maxServo = kXBusMaxServoNum; else if (maxServo == 0) @@ -98,6 +115,7 @@ bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1; // add 1 for CRC if (bufferSize < kCmdDataPacketSize) bufferSize = kCmdDataPacketSize; + chPacketBuffer = (uint8_t*)malloc(bufferSize); sendBuffer = (uint8_t*)malloc(bufferSize); @@ -108,37 +126,40 @@ chPacketBuffer[kCHDataPacketType] = 0x00; // initialize serial - RawSerial::baud(kXBusBaudrate); - RawSerial::format(8, RawSerial::None, 1); + XBusPort.baud(kXBusBaudrate); + XBusPort.format(8, RawSerial::None, 1); #if DEVICE_SERIAL_FC - RawSerial::set_flow_control(RawSerial::Disabled, NC, NC); + XBusPort.set_flow_control(RawSerial::Disabled, NC, NC); #endif - RawSerial::attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq); - RawSerial::attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq); -// DigitalInOut::mode(PullNone); -// DigitalInOut::input(); +#ifdef TARGET_KL25Z + // do nothing here +#else + XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq); +#endif + XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq); + serial_pinout_tx(txPin); + + return kXBusError_NoError; } - //**************************************************************************** -// XBusServo::~XBusServo +// XBusServo::stop // return : none // parameter : none // -// Destructor +// stop to use XBus // 2014/09/02 : move from Arduino lib by Sawa //**************************************************************************** -XBusServo::~XBusServo() +void XBusServo::stop() { - DBG("XBusServo::~XBusServo\n"); + DBG("XBusServo::stop\n"); free(chPacketBuffer); free(sendBuffer); } - //**************************************************************************** // XBusServo::write // return : none @@ -150,7 +171,7 @@ //**************************************************************************** void XBusServo::write(uint8_t* buffer, uint8_t length) { - // DBG("XBusServo::write\n"); +// DBG("XBusServo::write\n"); if (serialWriteBusy) return; @@ -159,11 +180,15 @@ XBusServo::sendLength = length; sendBufferPointer = buffer; - if (putc(*sendBufferPointer++) < 0) { + if (XBusPort.putc(*sendBufferPointer++) < 0) { serialWriteBusy = 0; XBusServo::sendLength = 0; - } else + } else { XBusServo::sendLength--; +#ifdef TARGET_KL25Z + XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq); +#endif + } } @@ -199,12 +224,18 @@ if (! serialWriteBusy) return; - if (XBusServo::sendLength <= 0) + if (XBusServo::sendLength <= 0) { serialWriteBusy = 0; - else { - if (putc(*sendBufferPointer++) < 0) { +#ifdef TARGET_KL25Z + XBusPort.attach(NULL, RawSerial::TxIrq); +#endif + } else { + if (XBusPort.putc(*sendBufferPointer++) < 0) { serialWriteBusy = 0; XBusServo::sendLength = 0; +#ifdef TARGET_KL25Z + XBusPort.attach(NULL, RawSerial::TxIrq); +#endif } else XBusServo::sendLength--; } @@ -223,7 +254,7 @@ { // DBG("XBusServo::RxIrqHandler\n"); - recieveBuffer[recieveBufferPointer++] = getc(); + recieveBuffer[recieveBufferPointer++] = XBusPort.getc(); if (recieveBufferPointer >= kRecieveBufferSize) recieveBufferPointer = 0; } @@ -466,7 +497,7 @@ { uint16_t dataOffset; - DBG("XBusServo::setServo\n"); +// DBG("XBusServo::setServo\n"); // convert to servo ID channelID &= 0x3F;