File content as of revision 4:b01a67ab40cf:

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


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';
    } else if( 0 == strncmp("^PR", strData, 3)) {
        AorR = 'R';
    }


    while ((p = strchr(strData, ','))!=NULL) *p = ' ';

    double a[3] = {0,0,0};
    if(*str != 'Z') {
        return;
    }

    sscanf((strData + 1), "%lf %lf %lf", &a[0], &a[1], &a[2]);

    if(AorR == 'A') {
        D_position[Z_x] = a[0];
        D_position[Z_y] = a[1];
        D_position[Z_z] = a[2];
    } else {
        D_position[Z_x] += a[0];
        D_position[Z_y] += a[1];
        D_position[Z_z] += a[2];
    }

    printf("x=%f y=%f z=%f \n", D_position[Z_x], D_position[Z_y], D_position[Z_z]);
    wait(0.1);
}

uint8_t MDX20::xyOrigin(void)
{
    uint8_t ans;
    char buffer[100];
    D_userOriginPosition[Z_x] = D_position[Z_x];
    D_userOriginPosition[Z_y] = D_position[Z_y];
    D_userOriginPosition[Z_z] = 0;

    sprintf(buffer, "!XO%d;",(int16_t)D_userOriginPosition[Z_x]);
//    printf("%s\r\n",buffer);
    sprintf(buffer, "!YO%d;",(int16_t)D_userOriginPosition[Z_y]);
//    printf("%s\r\n",buffer);

    return ans;
}

uint8_t MDX20::zOrigin(void)
{
    uint8_t ans;
    char buffer[100];

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

    return ans;
}

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

    while(_cts != 0) {}
    wait(0.1);  // このwait timeがないとMDX-20からのwait指示を読み飛ばす
//    printf("%s\r\n",data);
    _serial.printf("%s\r\n",data);

    integralPosition(data);
    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;
    ans &= sendData("^IN");
    ans &= sendData("!MC0");
    ans &= sendData("^PA");
    ans &= sendData("Z0,0,0");
    clearPositon();
    return (ans);
}

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

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

    ans &= sendData("^PA");

    sprintf(buffer, "Z%d,%d,%d",(int16_t)D_userOriginPosition[Z_x], (int16_t)D_userOriginPosition[Z_y], (int16_t)D_userOriginPosition[Z_z]);
//    printf("%s\r\n",buffer);
    ans &= sendData(buffer);

    return ans;
}

uint8_t MDX20::final(void)
{
    uint8_t ans;
    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[100];

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

    return (ans);
}