File content as of revision 1:4fdeb414f4ce:

/* 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 "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;


//
// 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()
{
    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()
{
    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);

            if (gDirty) {
                gOK2SendPacket = false;

                setCurrentValue();

                if (gSaveCurrentValue)
                    writeCurrentValue();

                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();
    }
}