main.cpp

00001 /* main.cpp file
00002  *
00003  * for testing mbed XBusServo.cpp
00004  *
00005  * Copyright (c) 2014-2014 JR PROPO
00006  * Released under the MIT License: http://mbed.org/license/mit
00007  *
00008  * by Zak Sawa
00009  */
00010 
00011 #include "mbed.h"
00012 #include "ACM1602NI.h"
00013 #include "XBusServo.h"
00014 
00015 //#define DEBUGmain
00016 
00017 #ifdef DEBUGmain
00018 #define DBG(fmt) printf(fmt)
00019 #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
00020 #else
00021 #define DBG(...)
00022 #define DBGF(...)
00023 #endif
00024 
00025 //
00026 // defines
00027 //
00028 #define kYellowSwitch       D2
00029 #define kRedSwitch          D3
00030 #define kBlueSwitch         D4
00031 #define kGreenSwitch        D5
00032 #define kRotary_A           D8
00033 #define kRotary_B           D9
00034 #define kXBusTx             PTE0    // PTD3 p13
00035 #define kXBusRx             PTE1    // PTD2 p14
00036 #define kXBusSwitch         D10
00037 #define kMaxServoNum        16      // 1 - 50
00038 #define kLCDI2C_CLK         PTC8
00039 #define lLCDI2C_DATA        PTC9
00040 
00041 #define kMaxServoCommand    (sizeof(commandData) / sizeof(servoCommandRec))
00042 
00043 
00044 //
00045 // typedefs
00046 //
00047 typedef enum {
00048     kCursor_ID,
00049     kCursor_SubID,
00050     kCursor_Function,
00051     kCursor_Param
00052 }
00053 cursorType;
00054 
00055 typedef struct servoCommandRec {
00056     char                    name[9];
00057     unsigned char           order;
00058     unsigned char           writeIndex;
00059     unsigned char           payloadSize;        // 5 for 2byte data / 4 for 1byte data
00060     unsigned char           hexData;            // true if the data should be show in Hex
00061     unsigned char           unsignedData;       // true if the data is unsigned
00062     long                    maxValue;
00063     long                    minValue;
00064 }
00065 servoCommandRec;
00066 
00067 typedef enum {
00068     kRotaly_Stop,
00069     kRotaly_Right,
00070     kRotaly_Left
00071 }
00072 rotalyType;
00073 
00074 
00075 //
00076 // servo command data base
00077 //
00078 static const servoCommandRec        commandData[] = {
00079 //  name        order                           write index               size hex unsin  max         mix
00080     {"Position",    0x00,                       0,                          0,  1,  1,  0x0FFFF,    0x0000},
00081     {"Servo ID",    0x00,                       0,                          0,  0,  0,  50,         1},
00082     {"SubID   ",    0x00,                       0,                          0,  0,  0,  3,          0},
00083     {"Version ",    kXBusOrder_2_Version,       0,                          5,  1,  1,  0,          0},
00084     {"Model No",    kXBusOrder_2_Product,       0,                          5,  1,  1,  0,          0},
00085     {"Reverse ",    kXBusOrder_2_Reverse,       kParamIdx_Reversed,         5,  0,  0,  1,          0},
00086     {"Neutral ",    kXBusOrder_2_Neutral,       kParamIdx_NeutralOffset,    5,  0,  0,  300,        -300},
00087     {"H-Travel",    kXBusOrder_2_H_Travel,      kParamIdx_TravelHigh,       5,  0,  0,  192,        0},
00088     {"L-Travel",    kXBusOrder_2_L_Travel,      kParamIdx_TravelLow,        5,  0,  0,  192,        0},
00089     {"H-Limit ",    kXBusOrder_2_H_Limit,       kParamIdx_LimitHigh,        5,  1,  1,  0x0FFFF,    0x0000},
00090     {"L-Limit ",    kXBusOrder_2_L_Limit,       kParamIdx_LimitLow,         5,  1,  1,  0x0FFFF,    0x0000},
00091     {"P-Gain  ",    kXBusOrder_1_P_Gain,        kParamIdx_PGainDiff,        4,  0,  0,  50,         -50},
00092     {"I-Gain  ",    kXBusOrder_1_I_Gain,        kParamIdx_IGainDiff,        4,  0,  0,  50,         -50},
00093     {"D-Gain  ",    kXBusOrder_1_D_Gain,        kParamIdx_DGainDiff,        4,  0,  0,  50,         -50},
00094     {"Int-Max ",    kXBusOrder_2_MaxInteger,    kParamIdx_MaxIntegerDiff,   5,  0,  0,  999,        -999},
00095     {"DeadBand",    kXBusOrder_1_DeadBand,      kParamIdx_DeadBandDiff,     4,  0,  0,  10,         -10},
00096     {"180deg  ",    kXBusOrder_1_Angle_180,     kParamIdx_Angle_180,        4,  0,  0,  1,          0},
00097     {"SpeedLim",    kXBusOrder_1_SpeedLimit,    kParamIdx_SpeedLimit,       4,  0,  0,  30,         0},
00098     {"StopMode",    kXBusOrder_1_StopMode,      kParamIdx_StopMode,         4,  0,  0,  1,          0},
00099     {"PowOffst",    kXBusOrder_2_PowerOffset,   kParamIdx_PWOffsetDiff,     5,  0,  0,  999,        -999},
00100     {"SlowStat",    kXBusOrder_1_SlowStart,     kParamIdx_SlowStart,        4,  0,  0,  1,          0},
00101     {"AlarmLv ",    kXBusOrder_1_AlarmLevel,    kParamIdx_AlarmLevel,       4,  0,  0,  99,         0},
00102     {"AlarmDly",    kXBusOrder_2_AlarmDelay,    kParamIdx_AlarmDelay,       5,  0,  0,  5000,       0},
00103     {"CurPossi",    kXBusOrder_2_CurrentPos,    0,                          5,  1,  1,  0,          0},
00104     {"CurPower",    kXBusOrder_1_CurrentPow,    0,                          4,  0,  0,  0,          0}
00105 };
00106 
00107 
00108 //
00109 // global vars
00110 //
00111 ACM1602NI                       gLCD(lLCDI2C_DATA, kLCDI2C_CLK);
00112 XBusServo                       gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum);
00113 Ticker                          gTimer;
00114 DigitalIn                       gUp_SW(kYellowSwitch, PullUp);
00115 DigitalIn                       gLeft_SW(kBlueSwitch, PullUp);
00116 DigitalIn                       gRight_SW(kRedSwitch, PullUp);
00117 DigitalIn                       gDown_SW(kGreenSwitch, PullUp);
00118 DigitalIn                       gRotary_A(kRotary_A, PullUp);
00119 InterruptIn                     gRotary_B(kRotary_B);
00120 
00121 uint8_t                         gDirty = true;          // true for first update
00122 uint8_t                         gSaveCurrentValue = false;
00123 uint8_t                         gOK2SendPacket = true;
00124 uint8_t                         gCurrentValueChanged = false;
00125 
00126 uint16_t                        gCurrentPos = kXbusServoNeutral;
00127 int32_t                         gCurrentValue = 0;
00128 cursorType                      gCurrentCursor = kCursor_ID;
00129 volatile rotalyType             gRotalyJob = kRotaly_Stop;
00130 uint8_t                         gCurrentFunction = 0;
00131 uint8_t                         gNextFunction = 0;
00132 
00133 uint8_t                         gCurrentServoID = 1;
00134 uint8_t                         gCurrentSubID = 0;
00135 
00136 
00137 void getRotary(void);
00138 
00139 
00140 
00141 //=============================================================
00142 // XbusIntervalHandler()
00143 //  14mSec interval handler
00144 //=============================================================
00145 void XbusIntervalHandler()
00146 {
00147     if (gOK2SendPacket)
00148         gXBus.sendChannelDataPacket();
00149 }
00150 
00151 
00152 //=============================================================
00153 // init()
00154 //  initialize all setup
00155 //=============================================================
00156 void init()
00157 {
00158     gLCD.cls();
00159     gLCD.locate(0, 0);
00160 
00161     gRotary_B.mode(PullUp);
00162     gRotary_B.rise(getRotary);
00163     gRotary_B.fall(getRotary);
00164     gRotary_B.enable_irq();
00165 }
00166 
00167 
00168 //=============================================================
00169 // rotaryRight()
00170 //  This is what to do when rotary encoder turn right
00171 //=============================================================
00172 void rotaryRight()
00173 {
00174     if (gDirty)
00175         return;
00176 
00177     switch (gCurrentCursor) {
00178         case kCursor_ID:
00179             if (gCurrentServoID < kXBusMaxServoNum) {
00180                 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));
00181                 gCurrentServoID++;
00182                 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
00183                 gDirty = true;
00184             }
00185             break;
00186 
00187         case kCursor_SubID:
00188             if (gCurrentSubID < 3) {
00189                 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));
00190                 gCurrentSubID++;
00191                 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
00192                 gDirty = true;
00193             }
00194             break;
00195 
00196         case kCursor_Function:
00197             if (gCurrentFunction < (kMaxServoCommand - 1)) {
00198                 gNextFunction = gCurrentFunction + 1;
00199                 gDirty = true;
00200             }
00201             break;
00202 
00203         case kCursor_Param:
00204             if (commandData[gCurrentFunction].maxValue == commandData[gCurrentFunction].minValue)
00205                 break;
00206 
00207             if (gCurrentFunction == 0) {
00208                 gCurrentPos += 200;
00209                 gDirty = true;
00210             } else if (gCurrentValue < commandData[gCurrentFunction].maxValue) {
00211                 gCurrentValue++;
00212                 gCurrentValueChanged = true;
00213                 gDirty = true;
00214             }
00215             break;
00216     }
00217 }
00218 
00219 
00220 //=============================================================
00221 // rotaryLeft()
00222 //  This is what to do when rotary encoder turn left
00223 //=============================================================
00224 void rotaryLeft()
00225 {
00226     if (gDirty)
00227         return;
00228 
00229     switch (gCurrentCursor) {
00230         case kCursor_ID:
00231             if (gCurrentServoID > 1) {
00232                 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));       // add first servo with channelID = 0x01
00233                 gCurrentServoID--;
00234                 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);         // add first servo with channelID = 0x01
00235                 gDirty = true;
00236             }
00237             break;
00238 
00239         case kCursor_SubID:
00240             if (gCurrentSubID > 0) {
00241                 gXBus.removeServo(ChannelID(gCurrentServoID, gCurrentSubID));       // add first servo with channelID = 0x01
00242                 gCurrentSubID--;
00243                 gXBus.addServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);         // add first servo with channelID = 0x01
00244                 gDirty = true;
00245             }
00246             break;
00247 
00248         case kCursor_Function:
00249             if (gCurrentFunction > 0) {
00250                 gNextFunction = gCurrentFunction - 1;
00251                 gDirty = true;
00252             }
00253             break;
00254 
00255         case kCursor_Param:
00256             if (commandData[gCurrentFunction].maxValue == commandData[gCurrentFunction].minValue)
00257                 break;
00258 
00259             if (gCurrentFunction == 0) {
00260                 gCurrentPos -= 200;
00261                 gDirty = true;
00262             } else if (gCurrentValue > commandData[gCurrentFunction].minValue) {
00263                 gCurrentValue--;
00264                 gCurrentValueChanged = true;
00265                 gDirty = true;
00266             }
00267             break;
00268     }
00269 }
00270 
00271 
00272 //=============================================================
00273 // getRotary()
00274 //  This is the handler to get rotary encoder
00275 //=============================================================
00276 void getRotary(void)
00277 {
00278     static unsigned char sOldRot = 0;
00279 
00280     if (! gRotary_A.read()) {
00281         // Check to start rotating
00282         if (gRotary_B.read())
00283             sOldRot = 'R'; // Right turn started
00284         else
00285             sOldRot = 'L'; // Left turn started
00286     } else {
00287         // Check to stop rotating
00288         if (gRotary_B.read()) {
00289             if (sOldRot == 'L') // It's still left turn
00290                 if (gRotalyJob == kRotaly_Stop)
00291                     gRotalyJob = kRotaly_Left;
00292         } else {
00293             if (sOldRot == 'R')  // It's still right turn
00294                 if (gRotalyJob == kRotaly_Stop)
00295                     gRotalyJob = kRotaly_Right;
00296         }
00297 
00298         sOldRot = 0;
00299     }
00300 }
00301 
00302 
00303 //=============================================================
00304 // updateLCD()
00305 //  update contents of LCD
00306 //=============================================================
00307 void updateLCD()
00308 {
00309     // update ID
00310     gLCD.locate(0, 0);
00311     gLCD.printf("XBus ID %02d-%02d", gCurrentServoID, gCurrentSubID);
00312     gLCD.printf("  ");
00313 
00314     // update function name
00315     gLCD.locate(0, 1);
00316     gLCD.printf(commandData[gCurrentFunction].name);
00317 
00318     // update current value
00319     gLCD.locate(9, 1);
00320     switch(gCurrentFunction) {
00321         case 0:         // position
00322             gLCD.printf("%04X", gCurrentPos);
00323             break;
00324 
00325         default:
00326             if (commandData[gCurrentFunction].hexData)
00327                 gLCD.printf("%04X", gCurrentValue);
00328             else if ((commandData[gCurrentFunction].maxValue == 1) && (commandData[gCurrentFunction].minValue == 0)) {
00329                 if (gCurrentValue == 1)
00330                     gLCD.printf("  on");
00331                 else
00332                     gLCD.printf(" off");
00333             } else
00334                 gLCD.printf("%4d", gCurrentValue);
00335     }
00336     gLCD.printf("  ");
00337 
00338     // update cursor
00339     switch(gCurrentCursor) {
00340         case kCursor_ID:
00341             gLCD.locate(9, 0);
00342             break;
00343 
00344         case kCursor_SubID:
00345             gLCD.locate(12, 0);
00346             break;
00347 
00348         case kCursor_Function:
00349             gLCD.locate(7, 1);
00350             break;
00351 
00352         case kCursor_Param:
00353             gLCD.locate(12, 1);
00354             break;
00355     }
00356 }
00357 
00358 
00359 //=============================================================
00360 // buttonHandler()
00361 //  get push button status and change flags
00362 //=============================================================
00363 void buttonHandler()
00364 {
00365     if (! gUp_SW.read()) {
00366         if (gCurrentCursor == kCursor_Function) {
00367             gCurrentCursor = kCursor_ID;
00368             gDirty = true;
00369         } else if (gCurrentCursor == kCursor_Param) {
00370             gCurrentCursor = kCursor_SubID;
00371             gDirty = true;
00372             gSaveCurrentValue = true;
00373         }
00374     } else if (! gDown_SW.read()) {
00375         if (gCurrentCursor == kCursor_ID) {
00376             gCurrentCursor = kCursor_Function;
00377             gDirty = true;
00378         } else if (gCurrentCursor == kCursor_SubID) {
00379             gCurrentCursor = kCursor_Param;
00380             gDirty = true;
00381         }
00382     } else if (! gRight_SW.read()) {
00383         if (gCurrentCursor == kCursor_ID) {
00384             gCurrentCursor = kCursor_SubID;
00385             gDirty = true;
00386         } else if (gCurrentCursor == kCursor_Function) {
00387             gCurrentCursor = kCursor_Param;
00388             gDirty = true;
00389         }
00390     } else if (! gLeft_SW.read()) {
00391         if (gCurrentCursor == kCursor_SubID) {
00392             gCurrentCursor = kCursor_ID;
00393             gDirty = true;
00394         } else if (gCurrentCursor == kCursor_Param) {
00395             gCurrentCursor = kCursor_Function;
00396             gDirty = true;
00397             gSaveCurrentValue = true;
00398         }
00399     }
00400 }
00401 
00402 
00403 //=============================================================
00404 // setCurrentValue()
00405 //  set current value to the servo for temp
00406 //=============================================================
00407 void setCurrentValue()
00408 {
00409     int16_t         theValue;
00410     XBusError       result;
00411 
00412     // set current value
00413     if (gCurrentValueChanged) {
00414         if (commandData[gCurrentFunction].writeIndex != 0) {
00415             theValue = gCurrentValue;
00416             for(;;) {
00417                 result = gXBus.setCommand(ChannelID(gCurrentServoID, gCurrentSubID), commandData[gCurrentFunction].order, &theValue);
00418                 wait_ms(10);
00419                 if (result == kXBusError_NoError)
00420                     break;
00421             }
00422         }
00423     }
00424 }
00425 
00426 
00427 //=============================================================
00428 // writeCurrentValue()
00429 //  write current value to the servo
00430 //=============================================================
00431 void writeCurrentValue()
00432 {
00433     XBusError       result;
00434     uint8_t         currentChannelID;
00435 
00436     currentChannelID = ChannelID(gCurrentServoID, gCurrentSubID);
00437 
00438     if (commandData[gCurrentFunction].writeIndex == 0) {
00439         // for change ID
00440         if ((gCurrentFunction == 1) || (gCurrentFunction == 2)) {
00441             int         newChannelID;
00442 
00443             if (gCurrentFunction == 1)
00444                 gCurrentServoID = gCurrentValue;
00445             else
00446                 gCurrentSubID = gCurrentValue;
00447 
00448             newChannelID = ChannelID(gCurrentServoID, gCurrentSubID);
00449 
00450             for (;;) {
00451                 result = gXBus.setChannelID(currentChannelID, newChannelID);
00452                 wait_ms(10);
00453                 if (result == kXBusError_NoError)
00454                     break;
00455             }
00456 
00457             gXBus.removeServo(currentChannelID);
00458             gXBus.addServo(newChannelID, gCurrentPos);
00459         }
00460     } else {
00461         int16_t         writeIndex;
00462 
00463         writeIndex = commandData[gCurrentFunction].writeIndex;
00464         for (;;) {
00465             result = gXBus.setCommand(currentChannelID, kXBusOrder_2_ParamWrite, &writeIndex);
00466             wait_ms(10);
00467             if (result == kXBusError_NoError)
00468                 break;
00469         }
00470     }
00471 }
00472 
00473 
00474 //=============================================================
00475 // getCurrentValue()
00476 //  get current value from the servo
00477 //=============================================================
00478 void getCurrentValue()
00479 {
00480     int16_t         theValue;
00481     XBusError       result;
00482 
00483     // get current value
00484     switch(gCurrentFunction) {
00485         case 0:
00486         case 1:
00487         case 2:
00488             // do nothing
00489             break;
00490 
00491         default:
00492             for (;;) {
00493                 result = gXBus.getCommand(ChannelID(gCurrentServoID, gCurrentSubID), commandData[gCurrentFunction].order, &theValue);
00494                 wait_ms(10);
00495                 if (result == kXBusError_NoError)
00496                     break;
00497             }
00498             gCurrentValue = theValue;
00499             if (commandData[gCurrentFunction].unsignedData)
00500                 gCurrentValue &= 0x0000FFFF;
00501             break;
00502     }
00503 }
00504 
00505 
00506 //=============================================================
00507 // main()
00508 //
00509 //=============================================================
00510 int main()
00511 {
00512     init();
00513 
00514     if (gXBus.start() == kXBusError_NoError) {
00515         gXBus.addServo(0x01, kXbusServoNeutral);
00516 
00517         gTimer.attach_us(&XbusIntervalHandler, kXBusStandardInterval * 1000);
00518 
00519         while(1) {
00520             buttonHandler();
00521 
00522             switch (gRotalyJob) {
00523                 case kRotaly_Right:
00524                     rotaryRight();
00525                     break;
00526 
00527                 case kRotaly_Left:
00528                     rotaryLeft();
00529                     break;
00530 
00531                 case kRotaly_Stop:
00532                 default:
00533                     break;          // Do nothing
00534             }
00535 
00536 //           gCurrentPos += 20;
00537 //            gXBus.setServo(0x01, gCurrentPos);
00538 //            wait_ms(10);
00539 
00540             if (gDirty) {
00541                 gOK2SendPacket = false;
00542 
00543                 setCurrentValue();
00544 
00545                 if (gSaveCurrentValue)
00546                     writeCurrentValue();
00547 
00548                 if (gNextFunction != gCurrentFunction) {
00549                     gCurrentFunction = gNextFunction;
00550                     if (gCurrentFunction == 1)
00551                         gCurrentValue = gCurrentServoID;
00552                     else if (gCurrentFunction == 2)
00553                         gCurrentValue = gCurrentSubID;
00554                     else
00555                         getCurrentValue();
00556                 }
00557 
00558                 updateLCD();
00559                 gXBus.setServo(ChannelID(gCurrentServoID, gCurrentSubID), gCurrentPos);
00560 
00561                 gOK2SendPacket = true;
00562             }
00563 
00564             gDirty = false;
00565             gSaveCurrentValue = false;
00566             gRotalyJob = kRotaly_Stop;
00567         }
00568 
00569         gXBus.stop();
00570     }
00571 }