Diff: MDX20.cpp

Revision:

2:8446eb1774c4

Parent:

1:1751fee99a0d

Child:

3:3bf2936b8d9b

--- a/MDX20.cpp	Sat Oct 15 23:32:34 2016 +0000
+++ b/MDX20.cpp	Sun Oct 16 12:42:34 2016 +0000
@@ -13,6 +13,25 @@
 {
 }
 
+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::integralPosition(char *data)
+{
+    
+    }
+
 /**
 * MDX-15/20へのデータ送信
 * @@para *data : データ一行の先頭アドレス
@@ -22,14 +41,12 @@
 {
     uint8_t ans = 0;    // 0:送信キャンセル 1:送信完了
 
-    if(_cts == 0) {
-        wait(0.1);  // このwait timeがないとMDX-20からのwait指示を読み飛ばす
-        printf("%s\r\n",data);
-        _serial.printf("%s\r\n",data);
-        ans = 1;
-    } else {
-        // nothing
-    }
+    while(_cts != 0) {}
+    wait(0.1);  // このwait timeがないとMDX-20からのwait指示を読み飛ばす
+//    printf("%s\r\n",data);
+    _serial.printf("%s\r\n",data);
+    ans = 1;
+
     return (ans);
 }
 
@@ -39,3 +56,47 @@
     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::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);
+}