Kazutaka "Zak" Sawa / XBusServo

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;