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上でのみ、動作確認しています

Files at this revision

API Documentation at this revision

Revision 18:75ddf12d93b6, committed 2014-11-05

Comitter:
sawa
Date:
Wed Nov 05 03:12:27 2014 +0000
Parent:
17:3ffb2e3e3bec
Commit message:
test publish

Changed in this revision

XBusServo.cpp Show annotated file Show diff for this revision Revisions of this file
XBusServo.h Show annotated file Show diff for this revision Revisions of this file 
--- a/XBusServo.cpp	Thu Oct 30 05:11:46 2014 +0000
+++ b/XBusServo.cpp	Wed Nov 05 03:12:27 2014 +0000
@@ -10,8 +10,6 @@
 
 #include "XBusServo.h"
 #include "pinmap.h"
-#include "gpio_api.h"
-
 
 
 #define kXBusBaudrate           250000          // bps
@@ -258,7 +256,6 @@
 //    2014/10/24 : add to setup reading for command data packet
 //****************************************************************************
 void XBusServo::TxIrqHandler(void)
-//void XBusServo::TxIrqHandler(MODSERIAL_IRQ_INFO *q)
 {
     int             result = 0;
 //    DBG("XBusServo::TxIrqHandler\n");
@@ -306,7 +303,6 @@
 //    2014/10/24 : modify to get the data from 1st byte of buffer
 //****************************************************************************
 void XBusServo::RxIrqHandler(void)
-//void XBusServo::RxIrqHandler(MODSERIAL_IRQ_INFO *q)
 {
 //    DBG("XBusServo::RxIrqHandler\n");
 
@@ -362,8 +358,8 @@
 XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
 {
     int                 sendSize;
-    int                 prevData = -1;
     uint8_t             dummy;
+    volatile uint8_t*   recieveDataPtr;
 
     DBG("XBusServo::sendCommandDataPacket\n");
 
@@ -394,22 +390,22 @@
     // send command
     sendSize = sendBuffer[kCmdDataPacketLength] + kPacketHeaderFooterSize;
     need2ReadData = ((! txOnly) && (channelID != 0));
-    recieveBuffer[sendSize + kCmdDataPacketLength] = 5;            // dummy to get real packet size
+    recieveDataPtr = &(recieveBuffer[sendSize]);
+    recieveDataPtr[kCmdDataPacketLength] = 5;            // dummy to get real packet size
     write(sendBuffer, sendSize);
 //    DBG("XBusServo::sendCommandDataPacket 2\n");
 
-    // if it's tx only mode or ID=0, it done
+    // if it's tx only mode or ID=0, it's done
     if (! need2ReadData) {
         flush();
         return kXBusError_NoError;
     }
 
 // wait to read all packet
-    while(recieveBufferPointer < (recieveBuffer[sendSize + kCmdDataPacketLength] + kPacketHeaderFooterSize + sendSize)) {
-        if (prevData != recieveBufferPointer) {
-            prevData = recieveBufferPointer;
-//           DBGF("XBusServo::sendCommandDataPacket 3 %02d %02X \n", recieveBufferPointer, recieveBuffer[recieveBufferPointer - 1]);
-        }
+    while(recieveBufferPointer < (recieveDataPtr[kCmdDataPacketLength] + kPacketHeaderFooterSize + sendSize)) {
+        if (recieveBufferPointer > 16)
+            break;                                  // some trouble happen
+            
         //                                           need to add time out
     }
 
@@ -421,26 +417,26 @@
     serialCommandBusy = false;
 
     // check CRC
-    if (crc8(recieveBuffer, recieveBuffer[kCHDataPacketLength] + 3) != 0) {
+    if (crc8((uint8_t*)recieveDataPtr, recieveDataPtr[kCHDataPacketLength] + 3) != 0) {
         DBG("XBusServo::sendCommandDataPacket kXBusError_CRCError\n");
         return kXBusError_CRCError;
     }
 
     // check unsupported
-    if (recieveBuffer[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported) {
+    if (recieveDataPtr[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported) {
         DBG("XBusServo::sendCommandDataPacket kXBusError_Unsupported\n");
         return kXBusError_Unsupported;
     }
 
     // send back the value
     if (valueSize == 1) {                   // 1 byte value
-        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
+        *value = recieveDataPtr[kCmdDataPacketData1];
         if (*value & 0x0080)
             *value |= 0xFF00;
     } else {
-        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
+        *value = recieveDataPtr[kCmdDataPacketData1];
         *value <<= 8;
-        *value |= recieveBuffer[sendSize + kCmdDataPacketData2];
+        *value |= recieveDataPtr[kCmdDataPacketData2];
     }
 
     return kXBusError_NoError;
@@ -652,7 +648,7 @@
 
     DBG("XBusServo::setChannelID\n");
 
-    if (txOnly)
+    if (! txOnly)
         return kXBusError_OnlyForTxOnlyMode;
 
     value = kXBusMode_IDSet;
@@ -677,9 +673,7 @@
 //****************************************************************************
 uint8_t XBusServo::getDataSize(uint8_t order)
 {
-    uint8_t        dataSize = 1;
-
-    DBG("XBusServo::getDataSize\n");
+ //   DBG("XBusServo::getDataSize\n");
 
     switch(order) {
         case kXBusOrder_2_Version:      // only for get
@@ -696,10 +690,10 @@
         case kXBusOrder_2_AlarmDelay:
         case kXBusOrder_2_CurrentPos:   // only for get
         case kXBusOrder_2_MaxInteger:
-            dataSize = 2;
+            return 2;
+        default:
+            return 1;
     }
-
-    return dataSize;
 }
 
 
@@ -753,7 +747,7 @@
 {
     DBG("XBusServo::setCommand\n");
 
-    if (txOnly)
+    if (! txOnly)
         return kXBusError_OnlyForTxOnlyMode;
 
     return sendCommandDataPacket(kXBusCmd_Set, 0, order, value, getDataSize(order));

--- a/XBusServo.h	Thu Oct 30 05:11:46 2014 +0000
+++ b/XBusServo.h	Wed Nov 05 03:12:27 2014 +0000
@@ -12,7 +12,7 @@
 #define XBusServo_h
 
 #include "mbed.h"
-//#include "MODSERIAL.h"
+
 
 /// standard interval time for XBus servo
 #define kXBusStandardInterval       14              // mSec
@@ -25,6 +25,9 @@
 /// maximum number of XBus servo
 #define kXBusMaxServoNum            50
 
+/// convert ServoID & SubID to real Channel ID
+#define ChannelID(ServoID, SubID)   ((ServoID & 0x3F) | ((SubID << 6) & 0xC0))
+
 
 //! XBus error code
 typedef enum {
@@ -242,7 +245,6 @@
 private:
 #define         kRecieveBufferSize          32
 
-//    MODSERIAL           XBusPort;
     RawSerial           XBusPort;
     DigitalOut          TxSwitch;
     Timeout             TxSwitchTimer;
@@ -255,7 +257,7 @@
     uint8_t*            sendBuffer;      // serial send buffer
     volatile uint8_t*   sendBufferPointer;
     volatile uint8_t    sendLength;
-    uint8_t             recieveBuffer[kRecieveBufferSize];   // serial recieve buffer
+    volatile uint8_t    recieveBuffer[kRecieveBufferSize];   // serial recieve buffer
     volatile int8_t     recieveBufferPointer;
     uint8_t             dirty;           //
     volatile uint8_t    serialCommandBusy; //