Revision 1:4fdeb414f4ce, committed 2014-11-05

Comitter:

sawa

Date:

Wed Nov 05 06:38:30 2014 +0000

Parent:

0:3e197b983b65

Commit message:

Test program to setup XBus servo settings.

Changed in this revision

diff -r 3e197b983b65 -r 4fdeb414f4ce ACM1602NI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ACM1602NI.lib	Wed Nov 05 06:38:30 2014 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/sawa/code/ACM1602NI/#1f671c868ce0

diff -r 3e197b983b65 -r 4fdeb414f4ce I2CLCD.lib
--- a/I2CLCD.lib	Thu Oct 02 08:47:49 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/okini3939/code/I2CLCD/#bc4583ce560e

diff -r 3e197b983b65 -r 4fdeb414f4ce XBusServo.lib
--- a/XBusServo.lib	Thu Oct 02 08:47:49 2014 +0000
+++ b/XBusServo.lib	Wed Nov 05 06:38:30 2014 +0000
@@ -1,1 +1,1 @@
-XBusServo#381d475cfd6c
+http://developer.mbed.org/users/sawa/code/XBusServo/#75ddf12d93b6

diff -r 3e197b983b65 -r 4fdeb414f4ce main.cpp
--- a/main.cpp	Thu Oct 02 08:47:49 2014 +0000
+++ b/main.cpp	Wed Nov 05 06:38:30 2014 +0000
@@ -1,40 +1,571 @@
-/* main.c file
+/* main.cpp file
  *
  * for testing mbed XBusServo.cpp
  *
  * Copyright (c) 2014-2014 JR PROPO
+ * Released under the MIT License: http://mbed.org/license/mit
+ *
  * by Zak Sawa
  */
 
 #include "mbed.h"
-//#include "I2CLCD.h"
+#include "ACM1602NI.h"
 #include "XBusServo.h"
 
+//#define DEBUGmain
+
+#ifdef DEBUGmain
+#define DBG(fmt) printf(fmt)
+#define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
+#else
+#define DBG(...)
+#define DBGF(...)
+#endif
+
+//
+// defines
+//
+#define kYellowSwitch       D2
+#define kRedSwitch          D3
+#define kBlueSwitch         D4
+#define kGreenSwitch        D5
+#define kRotary_A           D8
+#define kRotary_B           D9
+#define kXBusTx             PTE0    // PTD3 p13
+#define kXBusRx             PTE1    // PTD2 p14
+#define kXBusSwitch         D10
+#define kMaxServoNum        16      // 1 - 50
+#define kLCDI2C_CLK         PTC8
+#define lLCDI2C_DATA        PTC9
+
+#define kMaxServoCommand    (sizeof(commandData) / sizeof(servoCommandRec))
+
+
+//
+// typedefs
+//
+typedef enum {
+    kCursor_ID,
+    kCursor_SubID,
+    kCursor_Function,
+    kCursor_Param
+}
+cursorType;
+
+typedef struct servoCommandRec {
+    char                    name[9];
+    unsigned char           order;
+    unsigned char           writeIndex;
+    unsigned char           payloadSize;        // 5 for 2byte data / 4 for 1byte data
+    unsigned char           hexData;            // true if the data should be show in Hex
+    unsigned char           unsignedData;       // true if the data is unsigned
+    long                    maxValue;
+    long                    minValue;
+}
+servoCommandRec;
+
+typedef enum {
+    kRotaly_Stop,
+    kRotaly_Right,
+    kRotaly_Left
+}
+rotalyType;
+
 
-//I2CLCD      LCD(I2C_SDA, I2C_SCL);
-XBusServo   XBus(PTD3, PTD2, 10);
-Ticker      timer;
+//
+// servo command data base
+//
+static const servoCommandRec        commandData[] = {
+//  name        order                           write index               size hex unsin  max         mix
+    {"Position",    0x00,                       0,                          0,  1,  1,  0x0FFFF,    0x0000},
+    {"Servo ID",    0x00,                       0,                          0,  0,  0,  50,         1},
+    {"SubID   ",    0x00,                       0,                          0,  0,  0,  3,          0},
+    {"Version ",    kXBusOrder_2_Version,       0,                          5,  1,  1,  0,          0},
+    {"Model No",    kXBusOrder_2_Product,       0,                          5,  1,  1,  0,          0},
+    {"Reverse ",    kXBusOrder_2_Reverse,       kParamIdx_Reversed,         5,  0,  0,  1,          0},
+    {"Neutral ",    kXBusOrder_2_Neutral,       kParamIdx_NeutralOffset,    5,  0,  0,  300,        -300},
+    {"H-Travel",    kXBusOrder_2_H_Travel,      kParamIdx_TravelHigh,       5,  0,  0,  192,        0},
+    {"L-Travel",    kXBusOrder_2_L_Travel,      kParamIdx_TravelLow,        5,  0,  0,  192,        0},
+    {"H-Limit ",    kXBusOrder_2_H_Limit,       kParamIdx_LimitHigh,        5,  1,  1,  0x0FFFF,    0x0000},
+    {"L-Limit ",    kXBusOrder_2_L_Limit,       kParamIdx_LimitLow,         5,  1,  1,  0x0FFFF,    0x0000},
+    {"P-Gain  ",    kXBusOrder_1_P_Gain,        kParamIdx_PGainDiff,        4,  0,  0,  50,         -50},
+    {"I-Gain  ",    kXBusOrder_1_I_Gain,        kParamIdx_IGainDiff,        4,  0,  0,  50,         -50},
+    {"D-Gain  ",    kXBusOrder_1_D_Gain,        kParamIdx_DGainDiff,        4,  0,  0,  50,         -50},
+    {"Int-Max ",    kXBusOrder_2_MaxInteger,    kParamIdx_MaxIntegerDiff,   5,  0,  0,  999,        -999},
+    {"DeadBand",    kXBusOrder_1_DeadBand,      kParamIdx_DeadBandDiff,     4,  0,  0,  10,         -10},
+    {"180deg  ",    kXBusOrder_1_Angle_180,     kParamIdx_Angle_180,        4,  0,  0,  1,          0},
+    {"SpeedLim",    kXBusOrder_1_SpeedLimit,    kParamIdx_SpeedLimit,       4,  0,  0,  30,         0},
+    {"StopMode",    kXBusOrder_1_StopMode,      kParamIdx_StopMode,         4,  0,  0,  1,          0},
+    {"PowOffst",    kXBusOrder_2_PowerOffset,   kParamIdx_PWOffsetDiff,     5,  0,  0,  999,        -999},
+    {"SlowStat",    kXBusOrder_1_SlowStart,     kParamIdx_SlowStart,        4,  0,  0,  1,          0},
+    {"AlarmLv ",    kXBusOrder_1_AlarmLevel,    kParamIdx_AlarmLevel,       4,  0,  0,  99,         0},
+    {"AlarmDly",    kXBusOrder_2_AlarmDelay,    kParamIdx_AlarmDelay,       5,  0,  0,  5000,       0},
+    {"CurPossi",    kXBusOrder_2_CurrentPos,    0,                          5,  1,  1,  0,          0},
+    {"CurPower",    kXBusOrder_1_CurrentPow,    0,                          4,  0,  0,  0,          0}
+};
+
+
+//
+// global vars
+//
+ACM1602NI                       gLCD(lLCDI2C_DATA, kLCDI2C_CLK);
+XBusServo                       gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum);
+Ticker                          gTimer;
+DigitalIn                       gUp_SW(kYellowSwitch, PullUp);
+DigitalIn                       gLeft_SW(kBlueSwitch, PullUp);
+DigitalIn                       gRight_SW(kRedSwitch, PullUp);
+DigitalIn                       gDown_SW(kGreenSwitch, PullUp);
+DigitalIn                       gRotary_A(kRotary_A, PullUp);
+InterruptIn                     gRotary_B(kRotary_B);
+
+uint8_t                         gDirty = true;          // true for first update
+uint8_t                         gSaveCurrentValue = false;
+uint8_t                         gOK2SendPacket = true;
+uint8_t                         gCurrentValueChanged = false;
+
+uint16_t                        gCurrentPos = kXbusServoNeutral;
+int32_t                         gCurrentValue = 0;
+cursorType                      gCurrentCursor = kCursor_ID;
+volatile rotalyType             gRotalyJob = kRotaly_Stop;
+uint8_t                         gCurrentFunction = 0;
+uint8_t                         gNextFunction = 0;
+
+uint8_t                         gCurrentServoID = 1;
+uint8_t                         gCurrentSubID = 0;
+
+
+void getRotary(void);
 
 
 
+//=============================================================
+// XbusIntervalHandler()
+//  14mSec interval handler
+//=============================================================
 void XbusIntervalHandler()
 {
-    XBus.sendChannelDataPacket();
+    if (gOK2SendPacket)
+        gXBus.sendChannelDataPacket();
+}
+
+
+//=============================================================
+// init()
+//  initialize all setup
+//=============================================================
+void init()
+{
+    gLCD.cls();
+    gLCD.locate(0, 0);
+
+    gRotary_B.mode(PullUp);
+    gRotary_B.rise(getRotary);
+    gRotary_B.fall(getRotary);
+    gRotary_B.enable_irq();
+}
+
+
+//=============================================================
+// rotaryRight()
+//  This is what to do when rotary encoder turn right
+//=============================================================
+void rotaryRight()
+{
+    if (gDirty)
+        return;
+
+    switch (gCurrentCursor) {
+        case kCursor_ID:
+            if (gCurrentServoID < kXBusMaxServoNum) {
+                gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));
+                gCurrentServoID++;
+                gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
+                gDirty = true;
+            }
+            break;
+
+        case kCursor_SubID:
+            if (gCurrentSubID < 3) {
+                gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));
+                gCurrentSubID++;
+                gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
+                gDirty = true;
+            }
+            break;
+
+        case kCursor_Function:
+            if (gCurrentFunction < (kMaxServoCommand - 1)) {
+                gNextFunction = gCurrentFunction + 1;
+                gDirty = true;
+            }
+            break;
+
+        case kCursor_Param:
+            if (commandData[gCurrentFunction].maxValue == commandData[gCurrentFunction].minValue)
+                break;
+
+            if (gCurrentFunction == 0) {
+                gCurrentPos += 200;
+                gDirty = true;
+            } else if (gCurrentValue < commandData[gCurrentFunction].maxValue) {
+                gCurrentValue++;
+                gCurrentValueChanged = true;
+                gDirty = true;
+            }
+            break;
+    }
+}
+
+
+//=============================================================
+// rotaryLeft()
+//  This is what to do when rotary encoder turn left
+//=============================================================
+void rotaryLeft()
+{
+    if (gDirty)
+        return;
+
+    switch (gCurrentCursor) {
+        case kCursor_ID:
+            if (gCurrentServoID > 1) {
+                gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));       // add first servo with channelID = 0x01
+                gCurrentServoID--;
+                gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);         // add first servo with channelID = 0x01
+                gDirty = true;
+            }
+            break;
+
+        case kCursor_SubID:
+            if (gCurrentSubID > 0) {
+                gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));       // add first servo with channelID = 0x01
+                gCurrentSubID--;
+                gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);         // add first servo with channelID = 0x01
+                gDirty = true;
+            }
+            break;
+
+        case kCursor_Function:
+            if (gCurrentFunction > 0) {
+                gNextFunction = gCurrentFunction - 1;
+                gDirty = true;
+            }
+            break;
+
+        case kCursor_Param:
+            if (commandData[gCurrentFunction].maxValue == commandData[gCurrentFunction].minValue)
+                break;
+
+            if (gCurrentFunction == 0) {
+                gCurrentPos -= 200;
+                gDirty = true;
+            } else if (gCurrentValue > commandData[gCurrentFunction].minValue) {
+                gCurrentValue--;
+                gCurrentValueChanged = true;
+                gDirty = true;
+            }
+            break;
+    }
 }
 
 
+//=============================================================
+// getRotary()
+//  This is the handler to get rotary encoder
+//=============================================================
+void getRotary(void)
+{
+    static unsigned char sOldRot = 0;
 
+    if (! gRotary_A.read()) {
+        // Check to start rotating
+        if (gRotary_B.read())
+            sOldRot = 'R'; // Right turn started
+        else
+            sOldRot = 'L'; // Left turn started
+    } else {
+        // Check to stop rotating
+        if (gRotary_B.read()) {
+            if (sOldRot == 'L') // It's still left turn
+                if (gRotalyJob == kRotaly_Stop)
+                    gRotalyJob = kRotaly_Left;
+        } else {
+            if (sOldRot == 'R')  // It's still right turn
+                if (gRotalyJob == kRotaly_Stop)
+                    gRotalyJob = kRotaly_Right;
+        }
+
+        sOldRot = 0;
+    }
+}
+
+
+//=============================================================
+// updateLCD()
+//  update contents of LCD
+//=============================================================
+void updateLCD()
+{
+    // update ID
+    gLCD.locate(0, 0);
+    gLCD.printf("XBus ID %02d-%02d", gCurrentServoID, gCurrentSubID);
+    gLCD.printf("  ");
+
+    // update function name
+    gLCD.locate(0, 1);
+    gLCD.printf(commandData[gCurrentFunction].name);
+
+    // update current value
+    gLCD.locate(9, 1);
+    switch(gCurrentFunction) {
+        case 0:         // position
+            gLCD.printf("%04X", gCurrentPos);
+            break;
+
+        default:
+            if (commandData[gCurrentFunction].hexData)
+                gLCD.printf("%04X", gCurrentValue);
+            else if ((commandData[gCurrentFunction].maxValue == 1) && (commandData[gCurrentFunction].minValue == 0)) {
+                if (gCurrentValue == 1)
+                    gLCD.printf("  on");
+                else
+                    gLCD.printf(" off");
+            } else
+                gLCD.printf("%4d", gCurrentValue);
+    }
+    gLCD.printf("  ");
+
+    // update cursor
+    switch(gCurrentCursor) {
+        case kCursor_ID:
+            gLCD.locate(9, 0);
+            break;
+
+        case kCursor_SubID:
+            gLCD.locate(12, 0);
+            break;
+
+        case kCursor_Function:
+            gLCD.locate(7, 1);
+            break;
+
+        case kCursor_Param:
+            gLCD.locate(12, 1);
+            break;
+    }
+}
+
+
+//=============================================================
+// buttonHandler()
+//  get push button status and change flags
+//=============================================================
+void buttonHandler()
+{
+    if (! gUp_SW.read()) {
+        if (gCurrentCursor == kCursor_Function) {
+            gCurrentCursor = kCursor_ID;
+            gDirty = true;
+        } else if (gCurrentCursor == kCursor_Param) {
+            gCurrentCursor = kCursor_SubID;
+            gDirty = true;
+            gSaveCurrentValue = true;
+        }
+    } else if (! gDown_SW.read()) {
+        if (gCurrentCursor == kCursor_ID) {
+            gCurrentCursor = kCursor_Function;
+            gDirty = true;
+        } else if (gCurrentCursor == kCursor_SubID) {
+            gCurrentCursor = kCursor_Param;
+            gDirty = true;
+        }
+    } else if (! gRight_SW.read()) {
+        if (gCurrentCursor == kCursor_ID) {
+            gCurrentCursor = kCursor_SubID;
+            gDirty = true;
+        } else if (gCurrentCursor == kCursor_Function) {
+            gCurrentCursor = kCursor_Param;
+            gDirty = true;
+        }
+    } else if (! gLeft_SW.read()) {
+        if (gCurrentCursor == kCursor_SubID) {
+            gCurrentCursor = kCursor_ID;
+            gDirty = true;
+        } else if (gCurrentCursor == kCursor_Param) {
+            gCurrentCursor = kCursor_Function;
+            gDirty = true;
+            gSaveCurrentValue = true;
+        }
+    }
+}
+
+
+//=============================================================
+// setCurrentValue()
+//  set current value to the servo for temp
+//=============================================================
+void setCurrentValue()
+{
+    int16_t         theValue;
+    XBusError       result;
+
+    // set current value
+    if (gCurrentValueChanged) {
+        if (commandData[gCurrentFunction].writeIndex != 0) {
+            theValue = gCurrentValue;
+            for(;;) {
+                result = gXBus.setCommand(ChannelID(gCurrentServoID, gCurrentSubID), commandData[gCurrentFunction].order, &theValue);
+                wait_ms(10);
+                if (result == kXBusError_NoError)
+                    break;
+            }
+        }
+    }
+}
+
+
+//=============================================================
+// writeCurrentValue()
+//  write current value to the servo
+//=============================================================
+void writeCurrentValue()
+{
+    XBusError       result;
+    uint8_t         currentChannelID;
+
+    currentChannelID = ChannelID(gCurrentServoID, gCurrentSubID);
+
+    if (commandData[gCurrentFunction].writeIndex == 0) {
+        // for change ID
+        if ((gCurrentFunction == 1) || (gCurrentFunction == 2)) {
+            int         newChannelID;
+
+            if (gCurrentFunction == 1)
+                gCurrentServoID = gCurrentValue;
+            else
+                gCurrentSubID = gCurrentValue;
+
+            newChannelID = ChannelID(gCurrentServoID, gCurrentSubID);
+
+            for (;;) {
+                result = gXBus.setChannelID(currentChannelID, newChannelID);
+                wait_ms(10);
+                if (result == kXBusError_NoError)
+                    break;
+            }
+
+            gXBus.removeServo(currentChannelID);
+            gXBus.addServo(newChannelID, gCurrentPos);
+        }
+    } else {
+        int16_t         writeIndex;
+
+        writeIndex = commandData[gCurrentFunction].writeIndex;
+        for (;;) {
+            result = gXBus.setCommand(currentChannelID, kXBusOrder_2_ParamWrite, &writeIndex);
+            wait_ms(10);
+            if (result == kXBusError_NoError)
+                break;
+        }
+    }
+}
+
+
+//=============================================================
+// getCurrentValue()
+//  get current value from the servo
+//=============================================================
+void getCurrentValue()
+{
+    int16_t         theValue;
+    XBusError       result;
+
+    // get current value
+    switch(gCurrentFunction) {
+        case 0:
+        case 1:
+        case 2:
+            // do nothing
+            break;
+
+        default:
+            for (;;) {
+                result = gXBus.getCommand(ChannelID(gCurrentServoID, gCurrentSubID), commandData[gCurrentFunction].order, &theValue);
+                wait_ms(10);
+                if (result == kXBusError_NoError)
+                    break;
+            }
+            gCurrentValue = theValue;
+            if (commandData[gCurrentFunction].unsignedData)
+                gCurrentValue &= 0x0000FFFF;
+            break;
+    }
+}
+
+
+//=============================================================
+// main()
+//
+//=============================================================
 int main()
 {
-    uint16_t        theValue = kXbusServoNeutral;
+    init();
+
+    if (gXBus.start() == kXBusError_NoError) {
+        gXBus.addServo(0x01, kXbusServoNeutral);
+
+        gTimer.attach_us(&XbusIntervalHandler, kXBusStandardInterval * 1000);
+
+        while(1) {
+            buttonHandler();
+
+            switch (gRotalyJob) {
+                case kRotaly_Right:
+                    rotaryRight();
+                    break;
+
+                case kRotaly_Left:
+                    rotaryLeft();
+                    break;
+
+                case kRotaly_Stop:
+                default:
+                    break;          // Do nothing
+            }
+
+//           gCurrentPos += 20;
+//            gXBus.setServo(0x01, gCurrentPos);
+//            wait_ms(10);
 
-    XBus.addServo(0x01, kXbusServoNeutral);
-    
-    printf("added");
-//   timer.attach_us(&XbusIntervalHandler, kXBusInterval * 1000);
+            if (gDirty) {
+                gOK2SendPacket = false;
+
+                setCurrentValue();
+
+                if (gSaveCurrentValue)
+                    writeCurrentValue();
 
-//   while(1) {
-//       theValue += 10;
-//       XBus.setServo(0x01, theValue);
-//    }
+                if (gNextFunction != gCurrentFunction) {
+                    gCurrentFunction = gNextFunction;
+                    if (gCurrentFunction == 1)
+                        gCurrentValue = gCurrentServoID;
+                    else if (gCurrentFunction == 2)
+                        gCurrentValue = gCurrentSubID;
+                    else
+                        getCurrentValue();
+                }
+
+                updateLCD();
+                gXBus.setServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
+
+                gOK2SendPacket = true;
+            }
+
+            gDirty = false;
+            gSaveCurrentValue = false;
+            gRotalyJob = kRotaly_Stop;
+        }
+
+        gXBus.stop();
+    }
 }

diff -r 3e197b983b65 -r 4fdeb414f4ce mbed-src.lib
--- a/mbed-src.lib	Thu Oct 02 08:47:49 2014 +0000
+++ b/mbed-src.lib	Wed Nov 05 06:38:30 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed-src/#098575c6d2c8
+http://mbed.org/users/mbed_official/code/mbed-src/#be64abf45658