File content as of revision 9:5ee05e9c5aca:

#include "MDX20.h"
#include "BufferedSerial.h"

//#define DEBUG
#ifdef DEBUG
#define DEBUG_PRINT(...) printf(__VA_ARGS__)
#else
#define DEBUG_PRINT(...)
#endif // DEBUG


LocalFileSystem local("local");

MDX20::MDX20(PinName tx, PinName rx, PinName cts) : _serial(tx, rx), _cts(cts, PullUp)
{

    _serial.baud(9600);
    _serial.format(8,Serial::None,1);



}

MDX20::~MDX20()
{
}

void MDX20::clearPositon(void)
{
    D_position[Z_x] = 0;
    D_position[Z_y] = 0;
    D_position[Z_z] = 0;
}

void MDX20::answerPositon(int16_t *position)
{
    *(position + Z_x) = D_position[Z_x];
    *(position + Z_y) = D_position[Z_y];
    *(position + Z_z) = D_position[Z_z];

}

void MDX20::answerPositonMillimeter(float *position)
{
    *(position + Z_x) = (float)D_position[Z_x] * countToMillimeter;
    *(position + Z_y) = (float)D_position[Z_y] * countToMillimeter;
    *(position + Z_z) = (float)D_position[Z_z] * countToMillimeter;

}


void MDX20::integralPosition(char *str)
{
    char strData[100];
    char *p;
    static uint8_t AorR = 'A';  // 'A'=absolute 'R'=relative

    strcpy(strData, str);

    if( 0 == strncmp("^PA", strData, 3)) {
        AorR = 'A';
        return;
    } else if( 0 == strncmp("^PR", strData, 3)) {
        AorR = 'R';
        *(str + 2) = 'A';  // MDX-20に送信するときは相対値から絶対値にする(この関数の中で相対値から絶対値にしているため）
        return;
    }

    // コマンド中の","を" "に置き換える
    while ((p = strchr(strData, ','))!=NULL) *p = ' ';

    int16_t a[3] = {0,0,0};

    if(strncmp(str, "!ZZ",3) == 0) {
        sscanf((strData + 3), "%d %d %d", &a[0], &a[1], &a[2]);
    } else if(strncmp(str, "Z",1) == 0) {
        sscanf((strData + 1), "%d %d %d", &a[0], &a[1], &a[2]);
    } else if(strncmp(str, "^PU",3) == 0) {
        sscanf((strData + 3), "%d %d", &a[0], &a[1]);
        a[2] = SHRT_MAX;
    } else if(strncmp(str, "^PD",3) == 0) {
        sscanf((strData + 3), "%d %d", &a[0], &a[1]);
        a[2] = SHRT_MAX;
    } else if(strncmp(str, "!ZM",3) == 0) {
        sscanf((strData + 3), "%d", &a[2]);
        a[0] = SHRT_MAX;
        a[1] = SHRT_MAX;

    } else {
        return;
    }

    if(AorR == 'A') {
        if(a[0] != SHRT_MAX) {
            D_position[Z_x] = a[0] + D_userOriginPosition[Z_x];
        }
        if(a[1] != SHRT_MAX) {
            D_position[Z_y] = a[1] + D_userOriginPosition[Z_y];
        }
        if(a[2] != SHRT_MAX) {
            D_position[Z_z] = a[2] + D_userOriginPosition[Z_z];
        }
    } else {
        // controler axis move data change to absolute from relative
        if(a[0] != SHRT_MAX) {
            D_position[Z_x] += a[0];
        }
        if(a[1] != SHRT_MAX) {
            D_position[Z_y] += a[1];
        }
        if(a[2] != SHRT_MAX) {
            D_position[Z_z] += a[2];
        }
    }
    translationToControlerAxisMoveDataFromRMLAxisMoveData(str);
//    DEBUG_PRINT("x=%d y=%d z=%d \n", D_position[Z_x], D_position[Z_y], D_position[Z_z]);
//    wait(0.1);
}

void MDX20::translationToControlerAxisMoveDataFromRMLAxisMoveData(char *str)
{
    char buffer[50];

    if(strncmp(str, "!ZZ",3) == 0) {
        sprintf(buffer,"!ZZ%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
        strcpy(str, buffer);
    } else if(strncmp(str, "Z",1) == 0) {
        sprintf(buffer,"Z%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
        strcpy(str, buffer);
    } else if(strncmp(str, "^PU",3) == 0) {
        sprintf(buffer,"^PU%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
        strcpy(str, buffer);
    } else if(strncmp(str, "^PD",3) == 0) {
        sprintf(buffer,"^PD%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
        strcpy(str, buffer);
    } else if(strncmp(str, "!ZM",3) == 0) {
        sprintf(buffer,"!ZM%d;\r\n",D_position[Z_z]);
        strcpy(str, buffer);
    } else {
        // nothing
    }
}


uint8_t MDX20::xyOrigin(void)
{
    char buffer[50];
    uint8_t ans;
    D_userOriginPosition[Z_x] = D_position[Z_x];
    D_userOriginPosition[Z_y] = D_position[Z_y];
//    D_userOriginPosition[Z_z] = 0;
    FILE *fp;

    fp = fopen("/local/OriginX.ini", "w");
    sprintf(buffer,"%d",D_userOriginPosition[Z_x]);
    fprintf(fp, buffer);
    fclose(fp);

    fp = fopen("/local/OriginY.ini", "w");
    sprintf(buffer,"%d",D_userOriginPosition[Z_y]);
    fprintf(fp, buffer);
    fclose(fp);

    return ans;
}

uint8_t MDX20::zOrigin(void)
{
    char buffer[50];
    uint8_t ans;
    D_userOriginPosition[Z_z] = D_position[Z_z];

    FILE *fp = fopen("/local/OriginZ.ini", "w");
    sprintf(buffer,"%d",D_userOriginPosition[Z_z]);
    fprintf(fp, buffer);
    fclose(fp);

    /* Z0 command use
        sprintf(buffer, "!ZO%d;",0);    // 今いる位置をZ原点にするので"0"を設定する
    //    printf("%s\r\n",buffer);
        ans &= sendData(buffer);
    */
    return ans;
}

/** offset加算後の各軸の可動範囲チェック
 * @para data RMLデータ一命令文字列の先頭アドレス
 * @returns
 *  false :逸脱あり
 *  true  :正常
 */
int32_t MDX20::axisMovingCheck(char* data)
{
    int32_t ans = true;
    integralPosition(data);
    if(
        ((D_position[Z_x] < Z_xAxisMin) || (Z_xAxisMax < D_position[Z_x])) ||
        ((D_position[Z_y] < Z_yAxisMin) || (Z_yAxisMax < D_position[Z_y])) ||
        ((D_position[Z_z] < Z_zAxisMin) || (Z_zAxisMax < D_position[Z_z]))
    ) {
        ans = false;
    }
    return ans;
}

/**
* MDX-15/20へのデータ送信
* @@para *data : データ一行の先頭アドレス
* @@para uint8_t : 0:送信キャンセル 1:送信完了
*/
uint8_t MDX20::sendData(char* data)
{
    uint8_t ans = 0;    // 0:送信キャンセル 1:送信完了
    char buffer[50];
    strcpy(buffer, data);

    while(_cts != 0) {}
    wait(0.1);  // このwait timeがないとMDX-20からのwait指示を読み飛ばす

    if(strncmp(buffer, "!MC0", 4) == 0) {
        motorState = false;
    } else if(strncmp(buffer, "!MC1", 4) == 0) {
        motorState = true;
    } else {
        // nothing
    }

    DEBUG_PRINT("send = %s\r\n",buffer);
    integralPosition(buffer);
    _serial.printf("%s\r\n",buffer);

    // 可動範囲外に出たらmin,maxに差し替える
    if(D_position[Z_x] < Z_xAxisMin) {
        D_position[Z_x] = Z_xAxisMin;
    }
    if(Z_xAxisMax < D_position[Z_x]) {
        D_position[Z_x] = Z_xAxisMax;
    }
    if(D_position[Z_y] < Z_yAxisMin) {
        D_position[Z_y] = Z_yAxisMin;
    }
    if(Z_yAxisMax < D_position[Z_y]) {
        D_position[Z_y] = Z_yAxisMax;
    }
    if(D_position[Z_z] < Z_zAxisMin) {
        D_position[Z_z] = Z_zAxisMin;
    }
    if(Z_zAxisMax < D_position[Z_z]) {
        D_position[Z_z] = Z_zAxisMax;
    }

    ans = 1;

    return (ans);
}

/**
 *
 */
uint8_t MDX20::reciveData(void)
{
    char ans = 0;
    while(_serial.readable()) {
        ans = _serial.getc();
    }
    return ans;
}

int MDX20::putc(int c)
{
    _serial.putc(c);
    return c;
}

uint8_t MDX20::initial(void)
{
    uint8_t ans;
    char buffer[50];
    clearPositon();
    FILE *fp;

    fp = fopen("/local/ORIGINX.INI", "r");
    if ( fp != NULL ) {
        if(NULL != fgets(buffer, sizeof(buffer), fp)) {
            D_userOriginPosition[Z_x] = atoi(buffer);
        } else {
            // nothing
        }
        fclose(fp);

    } else {
        // nothing
    }

    fp = fopen("/local/ORIGINY.INI", "r");
    if ( fp != NULL ) {
        if(NULL != fgets(buffer, sizeof(buffer), fp)) {
            D_userOriginPosition[Z_y] = atoi(buffer);
        } else {
            // nothing
        }
        fclose(fp);


    } else {
        // nothing
    }

    fp = fopen("/local/ORIGINZ.INI", "r");
    if ( fp != NULL ) {
        if(NULL != fgets(buffer, sizeof(buffer), fp)) {
            D_userOriginPosition[Z_z] = atoi(buffer);
        } else {
            // nothing
        }
        fclose(fp);

    } else {
        // nothing
    }

    ans &= sendData("^IN;");
    ans &= sendData("!MC0;");
    ans &= sendData("!ZO0;");
    ans &= sendData("!ZO0;"); // Z axis origin initialaize
    ans &= sendData("^PA;");

    sprintf(buffer,"Z%d,%d,%d;",-D_userOriginPosition[Z_x], -D_userOriginPosition[Z_y], -D_userOriginPosition[Z_z]);

    ans &= sendData(buffer);

    return (ans);
}

uint8_t MDX20::motorOff(void)
{
    uint8_t ans;
    ans &= sendData("!MC0;");
    return (ans);
}

/**
 * @note モータを動かすためには位置設定コマンドの発行が必要
 */
uint8_t MDX20::motorOn(void)
{
    uint8_t ans;
    ans &= sendData("!MC1;");
    ans &= sendData("^PR;");
    ans &= sendData("Z0,0,0");
    return (ans);
}


uint8_t MDX20::userOriginInitial(void)
{
    char buffer[50];
    uint8_t ans;

    ans &= sendData("^PA;");
    ans &= sendData("!MC0;");

    sprintf(buffer, "Z%d,%d,%d;",0, 0, 0);
    ans &= sendData(buffer);

    return ans;
}

uint8_t MDX20::userXYOriginInitial(void)
{
    char buffer[50];
    uint8_t ans;

    ans &= sendData("^PA;");
    ans &= sendData("!MC0;");

    sprintf(buffer, "Z%d,%d,%d;",0, 0, D_position[Z_z] - D_userOriginPosition[Z_z]);
    ans &= sendData(buffer);

    return ans;
}

uint8_t MDX20::userZOriginInitial(void)
{
    char buffer[50];
    uint8_t ans;

    ans &= sendData("^PA;");
    ans &= sendData("!MC0;");

    sprintf(buffer, "!ZM%d;",0);
    ans &= sendData(buffer);

    return ans;
}

uint8_t MDX20::final(void)
{
    uint8_t ans;

// モータを止めて、Z軸を機械原点に戻す。(X,Y軸はその場にとどめる)
    ans &= sendData("!MC0;");
    ans &= sendData("^PA;");
    ans &= sendData("!ZO0;");
    ans &= sendData("!ZO0;"); // Z axis origin initialaize

/* このコードだとZ軸が原点に戻らないままXY実を原点に戻すためワークに引っ掛かり破損する
    ans &= sendData("!MC0;");
    ans &= sendData("^PA;");
    ans &= sendData("Z0,0,0;");
*/
    clearPositon();
    ans &= sendData("^IN;");
    return (ans);
}
/*
uint8_t MDX20::zeroSetting(void)
{
    uint8_t ans;

    ans &= sendData("!X00");
    ans &= sendData("!Y00");
    ans &= sendData("!Z00");
    clearPositon();
    return (ans);
}
*/

uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
{
    uint8_t ans;
    char buffer[50];

    ans &= sendData("!MC0;");
    strcpy(buffer, "^PR;");
    ans &= sendData(buffer);
    sprintf(buffer, "Z%05d,%05d,%05d;",x, y, z);
    ans &= sendData(buffer);

    return (ans);
}

void MDX20::offsetXAxisAdjustment(int16_t axisData)
{
    XYZMove(axisData, 0, 0);
//    D_userOriginPosition[Z_x] = 0;

}

void MDX20::offsetYAxisAdjustment(int16_t axisData)
{
    XYZMove(0, axisData, 0);
//    D_userOriginPosition[Z_y] = 0;

}

void MDX20::offsetZAxisAdjustment(int16_t axisData)
{
    XYZMove(0, 0, axisData);
//    D_userOriginPosition[Z_z] = 0;

}

void MDX20::userOriginPositionInitial(void)
{
    D_userOriginPosition[Z_x] = 0;
    D_userOriginPosition[Z_y] = 0;
    D_userOriginPosition[Z_z] = 0;
}

int32_t MDX20::motorStateCheck(void)
{
    return motorState;
}

/** MDX-20の接続確認
 * @return true:接続あり false:なし
 */
int32_t MDX20::connectCheck(void)
{
   int32_t ans = false;
   
   if(_cts == 0){
       ans = true;
       }else{
           ans = false;
        }
    return (ans);
    }