Revision 8:e65e532d1933, committed 2016-12-03

Comitter:

suupen

Date:

Sat Dec 03 13:21:20 2016 +0000

Parent:

7:54af1616e69e

Child:

9:5ee05e9c5aca

Commit message:

?????????

Changed in this revision

--- a/MDX20.cpp	Mon Nov 28 12:04:59 2016 +0000
+++ b/MDX20.cpp	Sat Dec 03 13:21:20 2016 +0000
@@ -1,7 +1,7 @@
 #include "MDX20.h"
 #include "BufferedSerial.h"
 
-#define DEBUG
+//#define DEBUG
 #ifdef DEBUG
 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
 #else
@@ -114,7 +114,7 @@
     }
     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);
+//    wait(0.1);
 }
 
 void MDX20::translationToControlerAxisMoveDataFromRMLAxisMoveData(char *str)
@@ -183,6 +183,26 @@
     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 : データ一行の先頭アドレス
@@ -209,6 +229,26 @@
     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);
@@ -300,9 +340,9 @@
 uint8_t MDX20::motorOn(void)
 {
     uint8_t ans;
-        ans &= sendData("!MC1;");
-        ans &= sendData("^PR;");
-        ans &= sendData("Z0,0,0");
+    ans &= sendData("!MC1;");
+    ans &= sendData("^PR;");
+    ans &= sendData("Z0,0,0");
     return (ans);
 }
 
@@ -389,5 +429,5 @@
 int32_t MDX20::motorStateCheck(void)
 {
     return motorState;
-    }
+}
 

--- a/MDX20.h	Mon Nov 28 12:04:59 2016 +0000
+++ b/MDX20.h	Sat Dec 03 13:21:20 2016 +0000
@@ -22,6 +22,8 @@
 
 
     uint8_t sendData(char* data);
+    int32_t axisMovingCheck(char* data);
+
     uint8_t reciveData(void);
 
     int putc(int c);
@@ -59,6 +61,16 @@
 #define Z_x (0)
 #define Z_y (1)
 #define Z_z (2)
+
+#define Z_xAxisMin (0)
+#define Z_xAxisMax (8128)
+
+#define Z_yAxisMin (0)
+#define Z_yAxisMax (6096)
+
+#define Z_zAxisMin (-2420)
+#define Z_zAxisMax (0)
+
 };
 
 #endif  //  MBED_MDX20_H
\ No newline at end of file