Kazutaka "Zak" Sawa / mbed_XBus_Test

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

main.cpp@1:4fdeb414f4ce, 2014-11-05 (annotated)

Committer:
sawa
Date:
Wed Nov 05 06:38:30 2014 +0000
Revision:
1:4fdeb414f4ce
Parent:
0:3e197b983b65 
Test program to setup XBus servo settings.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sawa 1:4fdeb414f4ce 1 /* main.cpp file
sawa 0:3e197b983b65 2 *
sawa 0:3e197b983b65 3 * for testing mbed XBusServo.cpp
sawa 0:3e197b983b65 4 *
sawa 0:3e197b983b65 5 * Copyright (c) 2014-2014 JR PROPO
sawa 1:4fdeb414f4ce 6 * Released under the MIT License: http://mbed.org/license/mit
sawa 1:4fdeb414f4ce 7 *
sawa 0:3e197b983b65 8 * by Zak Sawa
sawa 0:3e197b983b65 9 */
sawa 0:3e197b983b65 10
sawa 0:3e197b983b65 11 #include "mbed.h"
sawa 1:4fdeb414f4ce 12 #include "ACM1602NI.h"
sawa 0:3e197b983b65 13 #include "XBusServo.h"
sawa 0:3e197b983b65 14
sawa 1:4fdeb414f4ce 15 //#define DEBUGmain
sawa 1:4fdeb414f4ce 16
sawa 1:4fdeb414f4ce 17 #ifdef DEBUGmain
sawa 1:4fdeb414f4ce 18 #define DBG(fmt) printf(fmt)
sawa 1:4fdeb414f4ce 19 #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
sawa 1:4fdeb414f4ce 20 #else
sawa 1:4fdeb414f4ce 21 #define DBG(...)
sawa 1:4fdeb414f4ce 22 #define DBGF(...)
sawa 1:4fdeb414f4ce 23 #endif
sawa 1:4fdeb414f4ce 24
sawa 1:4fdeb414f4ce 25 //
sawa 1:4fdeb414f4ce 26 // defines
sawa 1:4fdeb414f4ce 27 //
sawa 1:4fdeb414f4ce 28 #define kYellowSwitch D2
sawa 1:4fdeb414f4ce 29 #define kRedSwitch D3
sawa 1:4fdeb414f4ce 30 #define kBlueSwitch D4
sawa 1:4fdeb414f4ce 31 #define kGreenSwitch D5
sawa 1:4fdeb414f4ce 32 #define kRotary_A D8
sawa 1:4fdeb414f4ce 33 #define kRotary_B D9
sawa 1:4fdeb414f4ce 34 #define kXBusTx PTE0 // PTD3 p13
sawa 1:4fdeb414f4ce 35 #define kXBusRx PTE1 // PTD2 p14
sawa 1:4fdeb414f4ce 36 #define kXBusSwitch D10
sawa 1:4fdeb414f4ce 37 #define kMaxServoNum 16 // 1 - 50
sawa 1:4fdeb414f4ce 38 #define kLCDI2C_CLK PTC8
sawa 1:4fdeb414f4ce 39 #define lLCDI2C_DATA PTC9
sawa 1:4fdeb414f4ce 40
sawa 1:4fdeb414f4ce 41 #define kMaxServoCommand (sizeof(commandData) / sizeof(servoCommandRec))
sawa 1:4fdeb414f4ce 42
sawa 1:4fdeb414f4ce 43
sawa 1:4fdeb414f4ce 44 //
sawa 1:4fdeb414f4ce 45 // typedefs
sawa 1:4fdeb414f4ce 46 //
sawa 1:4fdeb414f4ce 47 typedef enum {
sawa 1:4fdeb414f4ce 48 kCursor_ID,
sawa 1:4fdeb414f4ce 49 kCursor_SubID,
sawa 1:4fdeb414f4ce 50 kCursor_Function,
sawa 1:4fdeb414f4ce 51 kCursor_Param
sawa 1:4fdeb414f4ce 52 }
sawa 1:4fdeb414f4ce 53 cursorType;
sawa 1:4fdeb414f4ce 54
sawa 1:4fdeb414f4ce 55 typedef struct servoCommandRec {
sawa 1:4fdeb414f4ce 56 char name[9];
sawa 1:4fdeb414f4ce 57 unsigned char order;
sawa 1:4fdeb414f4ce 58 unsigned char writeIndex;
sawa 1:4fdeb414f4ce 59 unsigned char payloadSize; // 5 for 2byte data / 4 for 1byte data
sawa 1:4fdeb414f4ce 60 unsigned char hexData; // true if the data should be show in Hex
sawa 1:4fdeb414f4ce 61 unsigned char unsignedData; // true if the data is unsigned
sawa 1:4fdeb414f4ce 62 long maxValue;
sawa 1:4fdeb414f4ce 63 long minValue;
sawa 1:4fdeb414f4ce 64 }
sawa 1:4fdeb414f4ce 65 servoCommandRec;
sawa 1:4fdeb414f4ce 66
sawa 1:4fdeb414f4ce 67 typedef enum {
sawa 1:4fdeb414f4ce 68 kRotaly_Stop,
sawa 1:4fdeb414f4ce 69 kRotaly_Right,
sawa 1:4fdeb414f4ce 70 kRotaly_Left
sawa 1:4fdeb414f4ce 71 }
sawa 1:4fdeb414f4ce 72 rotalyType;
sawa 1:4fdeb414f4ce 73
sawa 0:3e197b983b65 74
sawa 1:4fdeb414f4ce 75 //
sawa 1:4fdeb414f4ce 76 // servo command data base
sawa 1:4fdeb414f4ce 77 //
sawa 1:4fdeb414f4ce 78 static const servoCommandRec commandData[] = {
sawa 1:4fdeb414f4ce 79 // name order write index size hex unsin max mix
sawa 1:4fdeb414f4ce 80 {"Position", 0x00, 0, 0, 1, 1, 0x0FFFF, 0x0000},
sawa 1:4fdeb414f4ce 81 {"Servo ID", 0x00, 0, 0, 0, 0, 50, 1},
sawa 1:4fdeb414f4ce 82 {"SubID ", 0x00, 0, 0, 0, 0, 3, 0},
sawa 1:4fdeb414f4ce 83 {"Version ", kXBusOrder_2_Version, 0, 5, 1, 1, 0, 0},
sawa 1:4fdeb414f4ce 84 {"Model No", kXBusOrder_2_Product, 0, 5, 1, 1, 0, 0},
sawa 1:4fdeb414f4ce 85 {"Reverse ", kXBusOrder_2_Reverse, kParamIdx_Reversed, 5, 0, 0, 1, 0},
sawa 1:4fdeb414f4ce 86 {"Neutral ", kXBusOrder_2_Neutral, kParamIdx_NeutralOffset, 5, 0, 0, 300, -300},
sawa 1:4fdeb414f4ce 87 {"H-Travel", kXBusOrder_2_H_Travel, kParamIdx_TravelHigh, 5, 0, 0, 192, 0},
sawa 1:4fdeb414f4ce 88 {"L-Travel", kXBusOrder_2_L_Travel, kParamIdx_TravelLow, 5, 0, 0, 192, 0},
sawa 1:4fdeb414f4ce 89 {"H-Limit ", kXBusOrder_2_H_Limit, kParamIdx_LimitHigh, 5, 1, 1, 0x0FFFF, 0x0000},
sawa 1:4fdeb414f4ce 90 {"L-Limit ", kXBusOrder_2_L_Limit, kParamIdx_LimitLow, 5, 1, 1, 0x0FFFF, 0x0000},
sawa 1:4fdeb414f4ce 91 {"P-Gain ", kXBusOrder_1_P_Gain, kParamIdx_PGainDiff, 4, 0, 0, 50, -50},
sawa 1:4fdeb414f4ce 92 {"I-Gain ", kXBusOrder_1_I_Gain, kParamIdx_IGainDiff, 4, 0, 0, 50, -50},
sawa 1:4fdeb414f4ce 93 {"D-Gain ", kXBusOrder_1_D_Gain, kParamIdx_DGainDiff, 4, 0, 0, 50, -50},
sawa 1:4fdeb414f4ce 94 {"Int-Max ", kXBusOrder_2_MaxInteger, kParamIdx_MaxIntegerDiff, 5, 0, 0, 999, -999},
sawa 1:4fdeb414f4ce 95 {"DeadBand", kXBusOrder_1_DeadBand, kParamIdx_DeadBandDiff, 4, 0, 0, 10, -10},
sawa 1:4fdeb414f4ce 96 {"180deg ", kXBusOrder_1_Angle_180, kParamIdx_Angle_180, 4, 0, 0, 1, 0},
sawa 1:4fdeb414f4ce 97 {"SpeedLim", kXBusOrder_1_SpeedLimit, kParamIdx_SpeedLimit, 4, 0, 0, 30, 0},
sawa 1:4fdeb414f4ce 98 {"StopMode", kXBusOrder_1_StopMode, kParamIdx_StopMode, 4, 0, 0, 1, 0},
sawa 1:4fdeb414f4ce 99 {"PowOffst", kXBusOrder_2_PowerOffset, kParamIdx_PWOffsetDiff, 5, 0, 0, 999, -999},
sawa 1:4fdeb414f4ce 100 {"SlowStat", kXBusOrder_1_SlowStart, kParamIdx_SlowStart, 4, 0, 0, 1, 0},
sawa 1:4fdeb414f4ce 101 {"AlarmLv ", kXBusOrder_1_AlarmLevel, kParamIdx_AlarmLevel, 4, 0, 0, 99, 0},
sawa 1:4fdeb414f4ce 102 {"AlarmDly", kXBusOrder_2_AlarmDelay, kParamIdx_AlarmDelay, 5, 0, 0, 5000, 0},
sawa 1:4fdeb414f4ce 103 {"CurPossi", kXBusOrder_2_CurrentPos, 0, 5, 1, 1, 0, 0},
sawa 1:4fdeb414f4ce 104 {"CurPower", kXBusOrder_1_CurrentPow, 0, 4, 0, 0, 0, 0}
sawa 1:4fdeb414f4ce 105 };
sawa 1:4fdeb414f4ce 106
sawa 1:4fdeb414f4ce 107
sawa 1:4fdeb414f4ce 108 //
sawa 1:4fdeb414f4ce 109 // global vars
sawa 1:4fdeb414f4ce 110 //
sawa 1:4fdeb414f4ce 111 ACM1602NI gLCD(lLCDI2C_DATA, kLCDI2C_CLK);
sawa 1:4fdeb414f4ce 112 XBusServo gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum);
sawa 1:4fdeb414f4ce 113 Ticker gTimer;
sawa 1:4fdeb414f4ce 114 DigitalIn gUp_SW(kYellowSwitch, PullUp);
sawa 1:4fdeb414f4ce 115 DigitalIn gLeft_SW(kBlueSwitch, PullUp);
sawa 1:4fdeb414f4ce 116 DigitalIn gRight_SW(kRedSwitch, PullUp);
sawa 1:4fdeb414f4ce 117 DigitalIn gDown_SW(kGreenSwitch, PullUp);
sawa 1:4fdeb414f4ce 118 DigitalIn gRotary_A(kRotary_A, PullUp);
sawa 1:4fdeb414f4ce 119 InterruptIn gRotary_B(kRotary_B);
sawa 1:4fdeb414f4ce 120
sawa 1:4fdeb414f4ce 121 uint8_t gDirty = true; // true for first update
sawa 1:4fdeb414f4ce 122 uint8_t gSaveCurrentValue = false;
sawa 1:4fdeb414f4ce 123 uint8_t gOK2SendPacket = true;
sawa 1:4fdeb414f4ce 124 uint8_t gCurrentValueChanged = false;
sawa 1:4fdeb414f4ce 125
sawa 1:4fdeb414f4ce 126 uint16_t gCurrentPos = kXbusServoNeutral;
sawa 1:4fdeb414f4ce 127 int32_t gCurrentValue = 0;
sawa 1:4fdeb414f4ce 128 cursorType gCurrentCursor = kCursor_ID;
sawa 1:4fdeb414f4ce 129 volatile rotalyType gRotalyJob = kRotaly_Stop;
sawa 1:4fdeb414f4ce 130 uint8_t gCurrentFunction = 0;
sawa 1:4fdeb414f4ce 131 uint8_t gNextFunction = 0;
sawa 1:4fdeb414f4ce 132
sawa 1:4fdeb414f4ce 133 uint8_t gCurrentServoID = 1;
sawa 1:4fdeb414f4ce 134 uint8_t gCurrentSubID = 0;
sawa 1:4fdeb414f4ce 135
sawa 1:4fdeb414f4ce 136
sawa 1:4fdeb414f4ce 137 void getRotary(void);
sawa 0:3e197b983b65 138
sawa 0:3e197b983b65 139
sawa 0:3e197b983b65 140
sawa 1:4fdeb414f4ce 141 //=============================================================
sawa 1:4fdeb414f4ce 142 // XbusIntervalHandler()
sawa 1:4fdeb414f4ce 143 // 14mSec interval handler
sawa 1:4fdeb414f4ce 144 //=============================================================
sawa 0:3e197b983b65 145 void XbusIntervalHandler()
sawa 0:3e197b983b65 146 {
sawa 1:4fdeb414f4ce 147 if (gOK2SendPacket)
sawa 1:4fdeb414f4ce 148 gXBus.sendChannelDataPacket();
sawa 1:4fdeb414f4ce 149 }
sawa 1:4fdeb414f4ce 150
sawa 1:4fdeb414f4ce 151
sawa 1:4fdeb414f4ce 152 //=============================================================
sawa 1:4fdeb414f4ce 153 // init()
sawa 1:4fdeb414f4ce 154 // initialize all setup
sawa 1:4fdeb414f4ce 155 //=============================================================
sawa 1:4fdeb414f4ce 156 void init()
sawa 1:4fdeb414f4ce 157 {
sawa 1:4fdeb414f4ce 158 gLCD.cls();
sawa 1:4fdeb414f4ce 159 gLCD.locate(0, 0);
sawa 1:4fdeb414f4ce 160
sawa 1:4fdeb414f4ce 161 gRotary_B.mode(PullUp);
sawa 1:4fdeb414f4ce 162 gRotary_B.rise(getRotary);
sawa 1:4fdeb414f4ce 163 gRotary_B.fall(getRotary);
sawa 1:4fdeb414f4ce 164 gRotary_B.enable_irq();
sawa 1:4fdeb414f4ce 165 }
sawa 1:4fdeb414f4ce 166
sawa 1:4fdeb414f4ce 167
sawa 1:4fdeb414f4ce 168 //=============================================================
sawa 1:4fdeb414f4ce 169 // rotaryRight()
sawa 1:4fdeb414f4ce 170 // This is what to do when rotary encoder turn right
sawa 1:4fdeb414f4ce 171 //=============================================================
sawa 1:4fdeb414f4ce 172 void rotaryRight()
sawa 1:4fdeb414f4ce 173 {
sawa 1:4fdeb414f4ce 174 if (gDirty)
sawa 1:4fdeb414f4ce 175 return;
sawa 1:4fdeb414f4ce 176
sawa 1:4fdeb414f4ce 177 switch (gCurrentCursor) {
sawa 1:4fdeb414f4ce 178 case kCursor_ID:
sawa 1:4fdeb414f4ce 179 if (gCurrentServoID < kXBusMaxServoNum) {
sawa 1:4fdeb414f4ce 180 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));
sawa 1:4fdeb414f4ce 181 gCurrentServoID++;
sawa 1:4fdeb414f4ce 182 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
sawa 1:4fdeb414f4ce 183 gDirty = true;
sawa 1:4fdeb414f4ce 184 }
sawa 1:4fdeb414f4ce 185 break;
sawa 1:4fdeb414f4ce 186
sawa 1:4fdeb414f4ce 187 case kCursor_SubID:
sawa 1:4fdeb414f4ce 188 if (gCurrentSubID < 3) {
sawa 1:4fdeb414f4ce 189 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));
sawa 1:4fdeb414f4ce 190 gCurrentSubID++;
sawa 1:4fdeb414f4ce 191 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
sawa 1:4fdeb414f4ce 192 gDirty = true;
sawa 1:4fdeb414f4ce 193 }
sawa 1:4fdeb414f4ce 194 break;
sawa 1:4fdeb414f4ce 195
sawa 1:4fdeb414f4ce 196 case kCursor_Function:
sawa 1:4fdeb414f4ce 197 if (gCurrentFunction < (kMaxServoCommand - 1)) {
sawa 1:4fdeb414f4ce 198 gNextFunction = gCurrentFunction + 1;
sawa 1:4fdeb414f4ce 199 gDirty = true;
sawa 1:4fdeb414f4ce 200 }
sawa 1:4fdeb414f4ce 201 break;
sawa 1:4fdeb414f4ce 202
sawa 1:4fdeb414f4ce 203 case kCursor_Param:
sawa 1:4fdeb414f4ce 204 if (commandData[gCurrentFunction].maxValue == commandData[gCurrentFunction].minValue)
sawa 1:4fdeb414f4ce 205 break;
sawa 1:4fdeb414f4ce 206
sawa 1:4fdeb414f4ce 207 if (gCurrentFunction == 0) {
sawa 1:4fdeb414f4ce 208 gCurrentPos += 200;
sawa 1:4fdeb414f4ce 209 gDirty = true;
sawa 1:4fdeb414f4ce 210 } else if (gCurrentValue < commandData[gCurrentFunction].maxValue) {
sawa 1:4fdeb414f4ce 211 gCurrentValue++;
sawa 1:4fdeb414f4ce 212 gCurrentValueChanged = true;
sawa 1:4fdeb414f4ce 213 gDirty = true;
sawa 1:4fdeb414f4ce 214 }
sawa 1:4fdeb414f4ce 215 break;
sawa 1:4fdeb414f4ce 216 }
sawa 1:4fdeb414f4ce 217 }
sawa 1:4fdeb414f4ce 218
sawa 1:4fdeb414f4ce 219
sawa 1:4fdeb414f4ce 220 //=============================================================
sawa 1:4fdeb414f4ce 221 // rotaryLeft()
sawa 1:4fdeb414f4ce 222 // This is what to do when rotary encoder turn left
sawa 1:4fdeb414f4ce 223 //=============================================================
sawa 1:4fdeb414f4ce 224 void rotaryLeft()
sawa 1:4fdeb414f4ce 225 {
sawa 1:4fdeb414f4ce 226 if (gDirty)
sawa 1:4fdeb414f4ce 227 return;
sawa 1:4fdeb414f4ce 228
sawa 1:4fdeb414f4ce 229 switch (gCurrentCursor) {
sawa 1:4fdeb414f4ce 230 case kCursor_ID:
sawa 1:4fdeb414f4ce 231 if (gCurrentServoID > 1) {
sawa 1:4fdeb414f4ce 232 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID)); // add first servo with channelID = 0x01
sawa 1:4fdeb414f4ce 233 gCurrentServoID--;
sawa 1:4fdeb414f4ce 234 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos); // add first servo with channelID = 0x01
sawa 1:4fdeb414f4ce 235 gDirty = true;
sawa 1:4fdeb414f4ce 236 }
sawa 1:4fdeb414f4ce 237 break;
sawa 1:4fdeb414f4ce 238
sawa 1:4fdeb414f4ce 239 case kCursor_SubID:
sawa 1:4fdeb414f4ce 240 if (gCurrentSubID > 0) {
sawa 1:4fdeb414f4ce 241 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID)); // add first servo with channelID = 0x01
sawa 1:4fdeb414f4ce 242 gCurrentSubID--;
sawa 1:4fdeb414f4ce 243 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos); // add first servo with channelID = 0x01
sawa 1:4fdeb414f4ce 244 gDirty = true;
sawa 1:4fdeb414f4ce 245 }
sawa 1:4fdeb414f4ce 246 break;
sawa 1:4fdeb414f4ce 247
sawa 1:4fdeb414f4ce 248 case kCursor_Function:
sawa 1:4fdeb414f4ce 249 if (gCurrentFunction > 0) {
sawa 1:4fdeb414f4ce 250 gNextFunction = gCurrentFunction - 1;
sawa 1:4fdeb414f4ce 251 gDirty = true;
sawa 1:4fdeb414f4ce 252 }
sawa 1:4fdeb414f4ce 253 break;
sawa 1:4fdeb414f4ce 254
sawa 1:4fdeb414f4ce 255 case kCursor_Param:
sawa 1:4fdeb414f4ce 256 if (commandData[gCurrentFunction].maxValue == commandData[gCurrentFunction].minValue)
sawa 1:4fdeb414f4ce 257 break;
sawa 1:4fdeb414f4ce 258
sawa 1:4fdeb414f4ce 259 if (gCurrentFunction == 0) {
sawa 1:4fdeb414f4ce 260 gCurrentPos -= 200;
sawa 1:4fdeb414f4ce 261 gDirty = true;
sawa 1:4fdeb414f4ce 262 } else if (gCurrentValue > commandData[gCurrentFunction].minValue) {
sawa 1:4fdeb414f4ce 263 gCurrentValue--;
sawa 1:4fdeb414f4ce 264 gCurrentValueChanged = true;
sawa 1:4fdeb414f4ce 265 gDirty = true;
sawa 1:4fdeb414f4ce 266 }
sawa 1:4fdeb414f4ce 267 break;
sawa 1:4fdeb414f4ce 268 }
sawa 0:3e197b983b65 269 }
sawa 0:3e197b983b65 270
sawa 0:3e197b983b65 271
sawa 1:4fdeb414f4ce 272 //=============================================================
sawa 1:4fdeb414f4ce 273 // getRotary()
sawa 1:4fdeb414f4ce 274 // This is the handler to get rotary encoder
sawa 1:4fdeb414f4ce 275 //=============================================================
sawa 1:4fdeb414f4ce 276 void getRotary(void)
sawa 1:4fdeb414f4ce 277 {
sawa 1:4fdeb414f4ce 278 static unsigned char sOldRot = 0;
sawa 0:3e197b983b65 279
sawa 1:4fdeb414f4ce 280 if (! gRotary_A.read()) {
sawa 1:4fdeb414f4ce 281 // Check to start rotating
sawa 1:4fdeb414f4ce 282 if (gRotary_B.read())
sawa 1:4fdeb414f4ce 283 sOldRot = 'R'; // Right turn started
sawa 1:4fdeb414f4ce 284 else
sawa 1:4fdeb414f4ce 285 sOldRot = 'L'; // Left turn started
sawa 1:4fdeb414f4ce 286 } else {
sawa 1:4fdeb414f4ce 287 // Check to stop rotating
sawa 1:4fdeb414f4ce 288 if (gRotary_B.read()) {
sawa 1:4fdeb414f4ce 289 if (sOldRot == 'L') // It's still left turn
sawa 1:4fdeb414f4ce 290 if (gRotalyJob == kRotaly_Stop)
sawa 1:4fdeb414f4ce 291 gRotalyJob = kRotaly_Left;
sawa 1:4fdeb414f4ce 292 } else {
sawa 1:4fdeb414f4ce 293 if (sOldRot == 'R') // It's still right turn
sawa 1:4fdeb414f4ce 294 if (gRotalyJob == kRotaly_Stop)
sawa 1:4fdeb414f4ce 295 gRotalyJob = kRotaly_Right;
sawa 1:4fdeb414f4ce 296 }
sawa 1:4fdeb414f4ce 297
sawa 1:4fdeb414f4ce 298 sOldRot = 0;
sawa 1:4fdeb414f4ce 299 }
sawa 1:4fdeb414f4ce 300 }
sawa 1:4fdeb414f4ce 301
sawa 1:4fdeb414f4ce 302
sawa 1:4fdeb414f4ce 303 //=============================================================
sawa 1:4fdeb414f4ce 304 // updateLCD()
sawa 1:4fdeb414f4ce 305 // update contents of LCD
sawa 1:4fdeb414f4ce 306 //=============================================================
sawa 1:4fdeb414f4ce 307 void updateLCD()
sawa 1:4fdeb414f4ce 308 {
sawa 1:4fdeb414f4ce 309 // update ID
sawa 1:4fdeb414f4ce 310 gLCD.locate(0, 0);
sawa 1:4fdeb414f4ce 311 gLCD.printf("XBus ID %02d-%02d", gCurrentServoID, gCurrentSubID);
sawa 1:4fdeb414f4ce 312 gLCD.printf(" ");
sawa 1:4fdeb414f4ce 313
sawa 1:4fdeb414f4ce 314 // update function name
sawa 1:4fdeb414f4ce 315 gLCD.locate(0, 1);
sawa 1:4fdeb414f4ce 316 gLCD.printf(commandData[gCurrentFunction].name);
sawa 1:4fdeb414f4ce 317
sawa 1:4fdeb414f4ce 318 // update current value
sawa 1:4fdeb414f4ce 319 gLCD.locate(9, 1);
sawa 1:4fdeb414f4ce 320 switch(gCurrentFunction) {
sawa 1:4fdeb414f4ce 321 case 0: // position
sawa 1:4fdeb414f4ce 322 gLCD.printf("%04X", gCurrentPos);
sawa 1:4fdeb414f4ce 323 break;
sawa 1:4fdeb414f4ce 324
sawa 1:4fdeb414f4ce 325 default:
sawa 1:4fdeb414f4ce 326 if (commandData[gCurrentFunction].hexData)
sawa 1:4fdeb414f4ce 327 gLCD.printf("%04X", gCurrentValue);
sawa 1:4fdeb414f4ce 328 else if ((commandData[gCurrentFunction].maxValue == 1) && (commandData[gCurrentFunction].minValue == 0)) {
sawa 1:4fdeb414f4ce 329 if (gCurrentValue == 1)
sawa 1:4fdeb414f4ce 330 gLCD.printf(" on");
sawa 1:4fdeb414f4ce 331 else
sawa 1:4fdeb414f4ce 332 gLCD.printf(" off");
sawa 1:4fdeb414f4ce 333 } else
sawa 1:4fdeb414f4ce 334 gLCD.printf("%4d", gCurrentValue);
sawa 1:4fdeb414f4ce 335 }
sawa 1:4fdeb414f4ce 336 gLCD.printf(" ");
sawa 1:4fdeb414f4ce 337
sawa 1:4fdeb414f4ce 338 // update cursor
sawa 1:4fdeb414f4ce 339 switch(gCurrentCursor) {
sawa 1:4fdeb414f4ce 340 case kCursor_ID:
sawa 1:4fdeb414f4ce 341 gLCD.locate(9, 0);
sawa 1:4fdeb414f4ce 342 break;
sawa 1:4fdeb414f4ce 343
sawa 1:4fdeb414f4ce 344 case kCursor_SubID:
sawa 1:4fdeb414f4ce 345 gLCD.locate(12, 0);
sawa 1:4fdeb414f4ce 346 break;
sawa 1:4fdeb414f4ce 347
sawa 1:4fdeb414f4ce 348 case kCursor_Function:
sawa 1:4fdeb414f4ce 349 gLCD.locate(7, 1);
sawa 1:4fdeb414f4ce 350 break;
sawa 1:4fdeb414f4ce 351
sawa 1:4fdeb414f4ce 352 case kCursor_Param:
sawa 1:4fdeb414f4ce 353 gLCD.locate(12, 1);
sawa 1:4fdeb414f4ce 354 break;
sawa 1:4fdeb414f4ce 355 }
sawa 1:4fdeb414f4ce 356 }
sawa 1:4fdeb414f4ce 357
sawa 1:4fdeb414f4ce 358
sawa 1:4fdeb414f4ce 359 //=============================================================
sawa 1:4fdeb414f4ce 360 // buttonHandler()
sawa 1:4fdeb414f4ce 361 // get push button status and change flags
sawa 1:4fdeb414f4ce 362 //=============================================================
sawa 1:4fdeb414f4ce 363 void buttonHandler()
sawa 1:4fdeb414f4ce 364 {
sawa 1:4fdeb414f4ce 365 if (! gUp_SW.read()) {
sawa 1:4fdeb414f4ce 366 if (gCurrentCursor == kCursor_Function) {
sawa 1:4fdeb414f4ce 367 gCurrentCursor = kCursor_ID;
sawa 1:4fdeb414f4ce 368 gDirty = true;
sawa 1:4fdeb414f4ce 369 } else if (gCurrentCursor == kCursor_Param) {
sawa 1:4fdeb414f4ce 370 gCurrentCursor = kCursor_SubID;
sawa 1:4fdeb414f4ce 371 gDirty = true;
sawa 1:4fdeb414f4ce 372 gSaveCurrentValue = true;
sawa 1:4fdeb414f4ce 373 }
sawa 1:4fdeb414f4ce 374 } else if (! gDown_SW.read()) {
sawa 1:4fdeb414f4ce 375 if (gCurrentCursor == kCursor_ID) {
sawa 1:4fdeb414f4ce 376 gCurrentCursor = kCursor_Function;
sawa 1:4fdeb414f4ce 377 gDirty = true;
sawa 1:4fdeb414f4ce 378 } else if (gCurrentCursor == kCursor_SubID) {
sawa 1:4fdeb414f4ce 379 gCurrentCursor = kCursor_Param;
sawa 1:4fdeb414f4ce 380 gDirty = true;
sawa 1:4fdeb414f4ce 381 }
sawa 1:4fdeb414f4ce 382 } else if (! gRight_SW.read()) {
sawa 1:4fdeb414f4ce 383 if (gCurrentCursor == kCursor_ID) {
sawa 1:4fdeb414f4ce 384 gCurrentCursor = kCursor_SubID;
sawa 1:4fdeb414f4ce 385 gDirty = true;
sawa 1:4fdeb414f4ce 386 } else if (gCurrentCursor == kCursor_Function) {
sawa 1:4fdeb414f4ce 387 gCurrentCursor = kCursor_Param;
sawa 1:4fdeb414f4ce 388 gDirty = true;
sawa 1:4fdeb414f4ce 389 }
sawa 1:4fdeb414f4ce 390 } else if (! gLeft_SW.read()) {
sawa 1:4fdeb414f4ce 391 if (gCurrentCursor == kCursor_SubID) {
sawa 1:4fdeb414f4ce 392 gCurrentCursor = kCursor_ID;
sawa 1:4fdeb414f4ce 393 gDirty = true;
sawa 1:4fdeb414f4ce 394 } else if (gCurrentCursor == kCursor_Param) {
sawa 1:4fdeb414f4ce 395 gCurrentCursor = kCursor_Function;
sawa 1:4fdeb414f4ce 396 gDirty = true;
sawa 1:4fdeb414f4ce 397 gSaveCurrentValue = true;
sawa 1:4fdeb414f4ce 398 }
sawa 1:4fdeb414f4ce 399 }
sawa 1:4fdeb414f4ce 400 }
sawa 1:4fdeb414f4ce 401
sawa 1:4fdeb414f4ce 402
sawa 1:4fdeb414f4ce 403 //=============================================================
sawa 1:4fdeb414f4ce 404 // setCurrentValue()
sawa 1:4fdeb414f4ce 405 // set current value to the servo for temp
sawa 1:4fdeb414f4ce 406 //=============================================================
sawa 1:4fdeb414f4ce 407 void setCurrentValue()
sawa 1:4fdeb414f4ce 408 {
sawa 1:4fdeb414f4ce 409 int16_t theValue;
sawa 1:4fdeb414f4ce 410 XBusError result;
sawa 1:4fdeb414f4ce 411
sawa 1:4fdeb414f4ce 412 // set current value
sawa 1:4fdeb414f4ce 413 if (gCurrentValueChanged) {
sawa 1:4fdeb414f4ce 414 if (commandData[gCurrentFunction].writeIndex != 0) {
sawa 1:4fdeb414f4ce 415 theValue = gCurrentValue;
sawa 1:4fdeb414f4ce 416 for(;;) {
sawa 1:4fdeb414f4ce 417 result = gXBus.setCommand(ChannelID(gCurrentServoID, gCurrentSubID), commandData[gCurrentFunction].order, &theValue);
sawa 1:4fdeb414f4ce 418 wait_ms(10);
sawa 1:4fdeb414f4ce 419 if (result == kXBusError_NoError)
sawa 1:4fdeb414f4ce 420 break;
sawa 1:4fdeb414f4ce 421 }
sawa 1:4fdeb414f4ce 422 }
sawa 1:4fdeb414f4ce 423 }
sawa 1:4fdeb414f4ce 424 }
sawa 1:4fdeb414f4ce 425
sawa 1:4fdeb414f4ce 426
sawa 1:4fdeb414f4ce 427 //=============================================================
sawa 1:4fdeb414f4ce 428 // writeCurrentValue()
sawa 1:4fdeb414f4ce 429 // write current value to the servo
sawa 1:4fdeb414f4ce 430 //=============================================================
sawa 1:4fdeb414f4ce 431 void writeCurrentValue()
sawa 1:4fdeb414f4ce 432 {
sawa 1:4fdeb414f4ce 433 XBusError result;
sawa 1:4fdeb414f4ce 434 uint8_t currentChannelID;
sawa 1:4fdeb414f4ce 435
sawa 1:4fdeb414f4ce 436 currentChannelID = ChannelID(gCurrentServoID, gCurrentSubID);
sawa 1:4fdeb414f4ce 437
sawa 1:4fdeb414f4ce 438 if (commandData[gCurrentFunction].writeIndex == 0) {
sawa 1:4fdeb414f4ce 439 // for change ID
sawa 1:4fdeb414f4ce 440 if ((gCurrentFunction == 1) || (gCurrentFunction == 2)) {
sawa 1:4fdeb414f4ce 441 int newChannelID;
sawa 1:4fdeb414f4ce 442
sawa 1:4fdeb414f4ce 443 if (gCurrentFunction == 1)
sawa 1:4fdeb414f4ce 444 gCurrentServoID = gCurrentValue;
sawa 1:4fdeb414f4ce 445 else
sawa 1:4fdeb414f4ce 446 gCurrentSubID = gCurrentValue;
sawa 1:4fdeb414f4ce 447
sawa 1:4fdeb414f4ce 448 newChannelID = ChannelID(gCurrentServoID, gCurrentSubID);
sawa 1:4fdeb414f4ce 449
sawa 1:4fdeb414f4ce 450 for (;;) {
sawa 1:4fdeb414f4ce 451 result = gXBus.setChannelID(currentChannelID, newChannelID);
sawa 1:4fdeb414f4ce 452 wait_ms(10);
sawa 1:4fdeb414f4ce 453 if (result == kXBusError_NoError)
sawa 1:4fdeb414f4ce 454 break;
sawa 1:4fdeb414f4ce 455 }
sawa 1:4fdeb414f4ce 456
sawa 1:4fdeb414f4ce 457 gXBus.removeServo(currentChannelID);
sawa 1:4fdeb414f4ce 458 gXBus.addServo(newChannelID, gCurrentPos);
sawa 1:4fdeb414f4ce 459 }
sawa 1:4fdeb414f4ce 460 } else {
sawa 1:4fdeb414f4ce 461 int16_t writeIndex;
sawa 1:4fdeb414f4ce 462
sawa 1:4fdeb414f4ce 463 writeIndex = commandData[gCurrentFunction].writeIndex;
sawa 1:4fdeb414f4ce 464 for (;;) {
sawa 1:4fdeb414f4ce 465 result = gXBus.setCommand(currentChannelID, kXBusOrder_2_ParamWrite, &writeIndex);
sawa 1:4fdeb414f4ce 466 wait_ms(10);
sawa 1:4fdeb414f4ce 467 if (result == kXBusError_NoError)
sawa 1:4fdeb414f4ce 468 break;
sawa 1:4fdeb414f4ce 469 }
sawa 1:4fdeb414f4ce 470 }
sawa 1:4fdeb414f4ce 471 }
sawa 1:4fdeb414f4ce 472
sawa 1:4fdeb414f4ce 473
sawa 1:4fdeb414f4ce 474 //=============================================================
sawa 1:4fdeb414f4ce 475 // getCurrentValue()
sawa 1:4fdeb414f4ce 476 // get current value from the servo
sawa 1:4fdeb414f4ce 477 //=============================================================
sawa 1:4fdeb414f4ce 478 void getCurrentValue()
sawa 1:4fdeb414f4ce 479 {
sawa 1:4fdeb414f4ce 480 int16_t theValue;
sawa 1:4fdeb414f4ce 481 XBusError result;
sawa 1:4fdeb414f4ce 482
sawa 1:4fdeb414f4ce 483 // get current value
sawa 1:4fdeb414f4ce 484 switch(gCurrentFunction) {
sawa 1:4fdeb414f4ce 485 case 0:
sawa 1:4fdeb414f4ce 486 case 1:
sawa 1:4fdeb414f4ce 487 case 2:
sawa 1:4fdeb414f4ce 488 // do nothing
sawa 1:4fdeb414f4ce 489 break;
sawa 1:4fdeb414f4ce 490
sawa 1:4fdeb414f4ce 491 default:
sawa 1:4fdeb414f4ce 492 for (;;) {
sawa 1:4fdeb414f4ce 493 result = gXBus.getCommand(ChannelID(gCurrentServoID, gCurrentSubID), commandData[gCurrentFunction].order, &theValue);
sawa 1:4fdeb414f4ce 494 wait_ms(10);
sawa 1:4fdeb414f4ce 495 if (result == kXBusError_NoError)
sawa 1:4fdeb414f4ce 496 break;
sawa 1:4fdeb414f4ce 497 }
sawa 1:4fdeb414f4ce 498 gCurrentValue = theValue;
sawa 1:4fdeb414f4ce 499 if (commandData[gCurrentFunction].unsignedData)
sawa 1:4fdeb414f4ce 500 gCurrentValue &= 0x0000FFFF;
sawa 1:4fdeb414f4ce 501 break;
sawa 1:4fdeb414f4ce 502 }
sawa 1:4fdeb414f4ce 503 }
sawa 1:4fdeb414f4ce 504
sawa 1:4fdeb414f4ce 505
sawa 1:4fdeb414f4ce 506 //=============================================================
sawa 1:4fdeb414f4ce 507 // main()
sawa 1:4fdeb414f4ce 508 //
sawa 1:4fdeb414f4ce 509 //=============================================================
sawa 0:3e197b983b65 510 int main()
sawa 0:3e197b983b65 511 {
sawa 1:4fdeb414f4ce 512 init();
sawa 1:4fdeb414f4ce 513
sawa 1:4fdeb414f4ce 514 if (gXBus.start() == kXBusError_NoError) {
sawa 1:4fdeb414f4ce 515 gXBus.addServo(0x01, kXbusServoNeutral);
sawa 1:4fdeb414f4ce 516
sawa 1:4fdeb414f4ce 517 gTimer.attach_us(&XbusIntervalHandler, kXBusStandardInterval * 1000);
sawa 1:4fdeb414f4ce 518
sawa 1:4fdeb414f4ce 519 while(1) {
sawa 1:4fdeb414f4ce 520 buttonHandler();
sawa 1:4fdeb414f4ce 521
sawa 1:4fdeb414f4ce 522 switch (gRotalyJob) {
sawa 1:4fdeb414f4ce 523 case kRotaly_Right:
sawa 1:4fdeb414f4ce 524 rotaryRight();
sawa 1:4fdeb414f4ce 525 break;
sawa 1:4fdeb414f4ce 526
sawa 1:4fdeb414f4ce 527 case kRotaly_Left:
sawa 1:4fdeb414f4ce 528 rotaryLeft();
sawa 1:4fdeb414f4ce 529 break;
sawa 1:4fdeb414f4ce 530
sawa 1:4fdeb414f4ce 531 case kRotaly_Stop:
sawa 1:4fdeb414f4ce 532 default:
sawa 1:4fdeb414f4ce 533 break; // Do nothing
sawa 1:4fdeb414f4ce 534 }
sawa 1:4fdeb414f4ce 535
sawa 1:4fdeb414f4ce 536 // gCurrentPos += 20;
sawa 1:4fdeb414f4ce 537 // gXBus.setServo(0x01, gCurrentPos);
sawa 1:4fdeb414f4ce 538 // wait_ms(10);
sawa 0:3e197b983b65 539
sawa 1:4fdeb414f4ce 540 if (gDirty) {
sawa 1:4fdeb414f4ce 541 gOK2SendPacket = false;
sawa 1:4fdeb414f4ce 542
sawa 1:4fdeb414f4ce 543 setCurrentValue();
sawa 1:4fdeb414f4ce 544
sawa 1:4fdeb414f4ce 545 if (gSaveCurrentValue)
sawa 1:4fdeb414f4ce 546 writeCurrentValue();
sawa 0:3e197b983b65 547
sawa 1:4fdeb414f4ce 548 if (gNextFunction != gCurrentFunction) {
sawa 1:4fdeb414f4ce 549 gCurrentFunction = gNextFunction;
sawa 1:4fdeb414f4ce 550 if (gCurrentFunction == 1)
sawa 1:4fdeb414f4ce 551 gCurrentValue = gCurrentServoID;
sawa 1:4fdeb414f4ce 552 else if (gCurrentFunction == 2)
sawa 1:4fdeb414f4ce 553 gCurrentValue = gCurrentSubID;
sawa 1:4fdeb414f4ce 554 else
sawa 1:4fdeb414f4ce 555 getCurrentValue();
sawa 1:4fdeb414f4ce 556 }
sawa 1:4fdeb414f4ce 557
sawa 1:4fdeb414f4ce 558 updateLCD();
sawa 1:4fdeb414f4ce 559 gXBus.setServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
sawa 1:4fdeb414f4ce 560
sawa 1:4fdeb414f4ce 561 gOK2SendPacket = true;
sawa 1:4fdeb414f4ce 562 }
sawa 1:4fdeb414f4ce 563
sawa 1:4fdeb414f4ce 564 gDirty = false;
sawa 1:4fdeb414f4ce 565 gSaveCurrentValue = false;
sawa 1:4fdeb414f4ce 566 gRotalyJob = kRotaly_Stop;
sawa 1:4fdeb414f4ce 567 }
sawa 1:4fdeb414f4ce 568
sawa 1:4fdeb414f4ce 569 gXBus.stop();
sawa 1:4fdeb414f4ce 570 }
sawa 0:3e197b983b65 571 }