Test probram to setup XBus servo settings.
Dependencies: ACM1602NI XBusServo mbed-src
This is just a working sample. This is setup tool for XBus servo. You can change all setting on XBus servo. Tested only on KL25Z
これはただの動作サンプルです。 これはXBusサーボ用のセットアップツールです。 このコードで、XBusサーボの全てのセッティングが変更できます。 KL25Z上でのみ動作確認しています。
Diff: main.cpp
- Revision:
- 1:4fdeb414f4ce
- Parent:
- 0:3e197b983b65
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(); + } }