Revision 5:bd414d8e483f, committed 2016-11-26

Comitter:

suupen

Date:

Sat Nov 26 06:41:51 2016 +0000

Parent:

4:b01a67ab40cf

Child:

6:b61356230acf

Commit message:

?????double??int????????; ;

Changed in this revision

--- a/MDX20.cpp	Wed Nov 23 09:12:01 2016 +0000
+++ b/MDX20.cpp	Sat Nov 26 06:41:51 2016 +0000
@@ -47,28 +47,43 @@
 
     if( 0 == strncmp("^PA", strData, 3)) {
         AorR = 'A';
+        return;
     } else if( 0 == strncmp("^PR", strData, 3)) {
         AorR = 'R';
+        return;
+    }
+
+    // コマンド中の","を" "に置き換える
+    while ((p = strchr(strData, ','))!=NULL) *p = ' ';
+
+    double a[3] = {0,0,0};
+
+    if(strncmp(str, "!ZZ",3) == 0) {
+        sscanf((strData + 3), "%lf %lf %lf", &a[0], &a[1], &a[2]);
+    } else if(strncmp(str, "Z",1) == 0) {
+        sscanf((strData + 1), "%lf %lf %lf", &a[0], &a[1], &a[2]);
+    } else if(strncmp(str, "^PU",3) == 0) {
+        sscanf((strData + 3), "%lf %lf", &a[0], &a[1]);
+        a[2] = DBL_MAX;
+    } else if(strncmp(str, "^PD",3) == 0) {
+        sscanf((strData + 3), "%lf %lf", &a[0], &a[1]);
+        a[2] = DBL_MAX;
     }
 
 
-    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];
+        if(a[2] != DBL_MAX){
         D_position[Z_z] = a[2];
+        }
     } else {
         D_position[Z_x] += a[0];
         D_position[Z_y] += a[1];
+        if(a[2] != DBL_MAX){
         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]);
@@ -81,7 +96,7 @@
     char buffer[100];
     D_userOriginPosition[Z_x] = D_position[Z_x];
     D_userOriginPosition[Z_y] = D_position[Z_y];
-    D_userOriginPosition[Z_z] = 0;
+//    D_userOriginPosition[Z_z] = 0;
 
     sprintf(buffer, "!XO%d;",(int16_t)D_userOriginPosition[Z_x]);
 //    printf("%s\r\n",buffer);
@@ -95,11 +110,14 @@
 {
     uint8_t ans;
     char buffer[100];
+    D_userOriginPosition[Z_z] = D_position[Z_z];
+    sprintf(buffer, "!ZO%d;",(int16_t)D_userOriginPosition[Z_z]);
 
-    sprintf(buffer, "!ZO%d;",0);    // 今いる位置をZ原点にするので"0"を設定する
-//    printf("%s\r\n",buffer);
-    ans &= sendData(buffer);
-
+    /* Z0 command use
+        sprintf(buffer, "!ZO%d;",0);    // 今いる位置をZ原点にするので"0"を設定する
+    //    printf("%s\r\n",buffer);
+        ans &= sendData(buffer);
+    */
     return ans;
 }
 
@@ -180,7 +198,7 @@
     ans &= sendData("^IN");
     return (ans);
 }
-
+/*
 uint8_t MDX20::zeroSetting(void)
 {
     uint8_t ans;
@@ -191,6 +209,8 @@
     clearPositon();
     return (ans);
 }
+*/
+
 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
 {
     uint8_t ans;

--- a/MDX20.h	Wed Nov 23 09:12:01 2016 +0000
+++ b/MDX20.h	Sat Nov 26 06:41:51 2016 +0000
@@ -3,6 +3,7 @@
 
 #include "mbed.h"
 #include "BufferedSerial.h"
+#include "float.h"
 class MDX20
 {
 public:
@@ -29,7 +30,7 @@
     uint8_t userOriginInitial(void);
 
     uint8_t final(void);
-    uint8_t zeroSetting(void);
+//    uint8_t zeroSetting(void);
     uint8_t XYZMove(int16_t x, int16_t y, int16_t z);
 
 private: