Revision 3:3bf2936b8d9b, committed 2016-11-19

Comitter:

suupen

Date:

Sat Nov 19 11:10:48 2016 +0000

Parent:

2:8446eb1774c4

Child:

4:b01a67ab40cf

Commit message:

a

Changed in this revision

--- a/MDX20.cpp	Sun Oct 16 12:42:34 2016 +0000
+++ b/MDX20.cpp	Sat Nov 19 11:10:48 2016 +0000
@@ -19,18 +19,89 @@
     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;
     }
 
-void MDX20::integralPosition(char *data)
+    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へのデータ送信
@@ -45,11 +116,22 @@
     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);
@@ -59,7 +141,7 @@
 uint8_t MDX20::initial(void)
 {
     uint8_t ans;
-    ans &= sendData(";;^IN");
+    ans &= sendData("^IN");
     ans &= sendData("!MC0");
     ans &= sendData("^PA");
     ans &= sendData("Z0,0,0");
@@ -67,16 +149,30 @@
     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;
+    uint8_t ans;
     ans &= sendData("!MC0");
     ans &= sendData("^PA");
     ans &= sendData("Z0,0,0");
     clearPositon();
-        ans &= sendData("^IN");
+    ans &= sendData("^IN");
     return (ans);
-    }
+}
 
 uint8_t MDX20::zeroSetting(void)
 {

--- a/MDX20.h	Sun Oct 16 12:42:34 2016 +0000
+++ b/MDX20.h	Sat Nov 19 11:10:48 2016 +0000
@@ -12,23 +12,34 @@
 
     void clearPositon(void);
     void answerPositon(int16_t *position);
-    void integralPosition(char *data);
-    
+    void answerPositonMillimeter(float *position);
+    void integralPosition(char *str);
+
+    uint8_t xyOrigin(void);
+    uint8_t zOrigin(void);
+
+
     uint8_t sendData(char* data);
+    uint8_t reciveData(void);
 
     int putc(int c);
     uint8_t initial(void);
+    uint8_t userOriginInitial(void);
+
     uint8_t final(void);
     uint8_t zeroSetting(void);
     uint8_t XYZMove(int16_t x, int16_t y, int16_t z);
 
 private:
+
     BufferedSerial _serial; // tx, rx(NC)
     DigitalIn _cts;
 
     char B_masterTx[0xff];
-    int16_t D_position[3]; //[0]:x, [1]:y, [2]:z
+    double D_position[3]; //[0]:x, [1]:y, [2]:z
+    double D_userOriginPosition[3]; //[0]:x, [1]:y, [2]:z
 
+#define countToMillimeter (0.025)
 #define Z_x (0)
 #define Z_y (1)
 #define Z_z (2)