Revision 6:b61356230acf, committed 2016-11-27

Comitter:

suupen

Date:

Sun Nov 27 12:14:47 2016 +0000

Parent:

5:bd414d8e483f

Child:

7:54af1616e69e

Commit message:

motor control??????????;

Changed in this revision

--- a/MDX20.cpp	Sat Nov 26 06:41:51 2016 +0000
+++ b/MDX20.cpp	Sun Nov 27 12:14:47 2016 +0000
@@ -1,12 +1,24 @@
 #include "MDX20.h"
 #include "BufferedSerial.h"
 
+#define DEBUG
+#ifdef DEBUG
+#define DEBUG_PRINT(...) printf(__VA_ARGS__)
+#else
+#define DEBUG_PRINT(...)
+#endif // DEBUG
+
+
+LocalFileSystem local("local");
 
 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()
@@ -50,68 +62,118 @@
         return;
     } else if( 0 == strncmp("^PR", strData, 3)) {
         AorR = 'R';
+        *(str + 2) = 'A';  // MDX-20に送信するときは相対値から絶対値にする(この関数の中で相対値から絶対値にしているため）
         return;
     }
 
     // コマンド中の","を" "に置き換える
     while ((p = strchr(strData, ','))!=NULL) *p = ' ';
 
-    double a[3] = {0,0,0};
+    int16_t a[3] = {0,0,0};
 
     if(strncmp(str, "!ZZ",3) == 0) {
-        sscanf((strData + 3), "%lf %lf %lf", &a[0], &a[1], &a[2]);
+        sscanf((strData + 3), "%d %d %d", &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]);
+        sscanf((strData + 1), "%d %d %d", &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;
+        sscanf((strData + 3), "%d %d", &a[0], &a[1]);
+        a[2] = SHRT_MAX;
     } else if(strncmp(str, "^PD",3) == 0) {
-        sscanf((strData + 3), "%lf %lf", &a[0], &a[1]);
-        a[2] = DBL_MAX;
+        sscanf((strData + 3), "%d %d", &a[0], &a[1]);
+        a[2] = SHRT_MAX;
+    } else if(strncmp(str, "!ZM",3) == 0) {
+        sscanf((strData + 3), "%d", &a[2]);
+        a[0] = SHRT_MAX;
+        a[1] = SHRT_MAX;
+
+    } else {
+        return;
     }
 
-
-
     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];
+        if(a[0] != SHRT_MAX) {
+            D_position[Z_x] = a[0] + D_userOriginPosition[Z_x];
+        }
+        if(a[1] != SHRT_MAX) {
+            D_position[Z_y] = a[1] + D_userOriginPosition[Z_y];
+        }
+        if(a[2] != SHRT_MAX) {
+            D_position[Z_z] = a[2] + D_userOriginPosition[Z_z];
         }
     } else {
-        D_position[Z_x] += a[0];
-        D_position[Z_y] += a[1];
-        if(a[2] != DBL_MAX){
-        D_position[Z_z] += a[2];
+        // controler axis move data change to absolute from relative
+        if(a[0] != SHRT_MAX) {
+            D_position[Z_x] += a[0];
+        }
+        if(a[1] != SHRT_MAX) {
+            D_position[Z_y] += a[1];
+        }
+        if(a[2] != SHRT_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]);
+    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);
 }
 
+void MDX20::translationToControlerAxisMoveDataFromRMLAxisMoveData(char *str)
+{
+    char buffer[50];
+
+    if(strncmp(str, "!ZZ",3) == 0) {
+        sprintf(buffer,"!ZZ%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
+        strcpy(str, buffer);
+    } else if(strncmp(str, "Z",1) == 0) {
+        sprintf(buffer,"Z%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
+        strcpy(str, buffer);
+    } else if(strncmp(str, "^PU",3) == 0) {
+        sprintf(buffer,"^PU%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
+        strcpy(str, buffer);
+    } else if(strncmp(str, "^PD",3) == 0) {
+        sprintf(buffer,"^PD%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
+        strcpy(str, buffer);
+    } else if(strncmp(str, "!ZM",3) == 0) {
+        sprintf(buffer,"!ZM%d;\r\n",D_position[Z_z]);
+        strcpy(str, buffer);
+    } else {
+        // nothing
+    }
+}
+
+
 uint8_t MDX20::xyOrigin(void)
 {
+    char buffer[50];
     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;
+    FILE *fp;
 
-    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);
+    fp = fopen("/local/OriginX.ini", "w");
+    sprintf(buffer,"%d",D_userOriginPosition[Z_x]);
+    fprintf(fp, buffer);
+    fclose(fp);
+
+    fp = fopen("/local/OriginY.ini", "w");
+    sprintf(buffer,"%d",D_userOriginPosition[Z_y]);
+    fprintf(fp, buffer);
+    fclose(fp);
 
     return ans;
 }
 
 uint8_t MDX20::zOrigin(void)
 {
+    char buffer[50];
     uint8_t ans;
-    char buffer[100];
     D_userOriginPosition[Z_z] = D_position[Z_z];
-    sprintf(buffer, "!ZO%d;",(int16_t)D_userOriginPosition[Z_z]);
+
+    FILE *fp = fopen("/local/OriginZ.ini", "w");
+    sprintf(buffer,"%d",D_userOriginPosition[Z_z]);
+    fprintf(fp, buffer);
+    fclose(fp);
 
     /* Z0 command use
         sprintf(buffer, "!ZO%d;",0);    // 今いる位置をZ原点にするので"0"を設定する
@@ -129,18 +191,30 @@
 uint8_t MDX20::sendData(char* data)
 {
     uint8_t ans = 0;    // 0:送信キャンセル 1:送信完了
+    char buffer[50];
+    strcpy(buffer, data);
 
     while(_cts != 0) {}
     wait(0.1);  // このwait timeがないとMDX-20からのwait指示を読み飛ばす
-//    printf("%s\r\n",data);
-    _serial.printf("%s\r\n",data);
 
-    integralPosition(data);
+    if(strncmp(buffer, "!MC0", 4) == 0) {
+        motorState = false;
+    } else if(strncmp(buffer, "!MC1", 4) == 0) {
+        motorState = true;
+    } else {
+        // nothing
+    }
+
+    DEBUG_PRINT("send = %s\r\n",buffer);
+    integralPosition(buffer);
+    _serial.printf("%s\r\n",buffer);
+
     ans = 1;
 
     return (ans);
 }
 
+
 uint8_t MDX20::reciveData(void)
 {
     char ans = 0;
@@ -159,30 +233,87 @@
 uint8_t MDX20::initial(void)
 {
     uint8_t ans;
-    ans &= sendData("^IN");
-    ans &= sendData("!MC0");
-    ans &= sendData("^PA");
-    ans &= sendData("Z0,0,0");
+    char buffer[50];
     clearPositon();
+    FILE *fp;
+
+    fp = fopen("/local/ORIGINX.INI", "r");
+    if ( fp != NULL ) {
+        if(NULL != fgets(buffer, sizeof(buffer), fp)) {
+            D_userOriginPosition[Z_x] = atoi(buffer);
+        } else {
+            // nothing
+        }
+        fclose(fp);
+
+    } else {
+        // nothing
+    }
+
+    fp = fopen("/local/ORIGINY.INI", "r");
+    if ( fp != NULL ) {
+        if(NULL != fgets(buffer, sizeof(buffer), fp)) {
+            D_userOriginPosition[Z_y] = atoi(buffer);
+        } else {
+            // nothing
+        }
+        fclose(fp);
+
+
+    } else {
+        // nothing
+    }
+
+    fp = fopen("/local/ORIGINZ.INI", "r");
+    if ( fp != NULL ) {
+        if(NULL != fgets(buffer, sizeof(buffer), fp)) {
+            D_userOriginPosition[Z_z] = atoi(buffer);
+        } else {
+            // nothing
+        }
+        fclose(fp);
+
+    } else {
+        // nothing
+    }
+
+    ans &= sendData("^IN;");
+    ans &= sendData("!MC0;");
+    ans &= sendData("!ZO0;");
+    ans &= sendData("!ZO0;"); // Z axis origin initialaize
+    ans &= sendData("^PA;");
+
+    sprintf(buffer,"Z%d,%d,%d;",-D_userOriginPosition[Z_x], -D_userOriginPosition[Z_y], -D_userOriginPosition[Z_z]);
+
+    ans &= sendData(buffer);
+
     return (ans);
 }
 
 uint8_t MDX20::motorOff(void)
 {
     uint8_t ans;
-    ans &= sendData("!MC0");
+    ans &= sendData("!MC0;");
     return (ans);
 }
 
+uint8_t MDX20::motorOn(void)
+{
+    uint8_t ans;
+        ans &= sendData("!MC1;");
+    return (ans);
+}
+
+
 uint8_t MDX20::userOriginInitial(void)
 {
-    char buffer[100];
+    char buffer[50];
     uint8_t ans;
 
-    ans &= sendData("^PA");
+    ans &= sendData("^PA;");
+    ans &= sendData("!MC0;");
 
-    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);
+    sprintf(buffer, "Z%d,%d,%d;",0, 0, 0);
     ans &= sendData(buffer);
 
     return ans;
@@ -191,11 +322,11 @@
 uint8_t MDX20::final(void)
 {
     uint8_t ans;
-    ans &= sendData("!MC0");
-    ans &= sendData("^PA");
-    ans &= sendData("Z0,0,0");
+    ans &= sendData("!MC0;");
+    ans &= sendData("^PA;");
+    ans &= sendData("Z0,0,0;");
     clearPositon();
-    ans &= sendData("^IN");
+    ans &= sendData("^IN;");
     return (ans);
 }
 /*
@@ -214,12 +345,47 @@
 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
 {
     uint8_t ans;
-    char buffer[100];
+    char buffer[50];
 
-    ans &= sendData("!MC0");
-    ans &= sendData("^PR");
-    sprintf(buffer, "Z%05d,%05d,%05d",x, y, z);
+    ans &= sendData("!MC0;");
+    strcpy(buffer, "^PR;");
+    ans &= sendData(buffer);
+    sprintf(buffer, "Z%05d,%05d,%05d;",x, y, z);
     ans &= sendData(buffer);
 
     return (ans);
 }
+
+void MDX20::offsetXAxisAdjustment(int16_t axisData)
+{
+    XYZMove(axisData, 0, 0);
+//    D_userOriginPosition[Z_x] = 0;
+
+}
+
+void MDX20::offsetYAxisAdjustment(int16_t axisData)
+{
+    XYZMove(0, axisData, 0);
+//    D_userOriginPosition[Z_y] = 0;
+
+}
+
+void MDX20::offsetZAxisAdjustment(int16_t axisData)
+{
+    XYZMove(0, 0, axisData);
+//    D_userOriginPosition[Z_z] = 0;
+
+}
+
+void MDX20::userOriginPositionInitial(void)
+{
+    D_userOriginPosition[Z_x] = 0;
+    D_userOriginPosition[Z_y] = 0;
+    D_userOriginPosition[Z_z] = 0;
+}
+
+int32_t MDX20::motorStateCheck(void)
+{
+    return motorState;
+    }
+

--- a/MDX20.h	Sat Nov 26 06:41:51 2016 +0000
+++ b/MDX20.h	Sun Nov 27 12:14:47 2016 +0000
@@ -3,7 +3,7 @@
 
 #include "mbed.h"
 #include "BufferedSerial.h"
-#include "float.h"
+#include "limits.h"
 class MDX20
 {
 public:
@@ -15,6 +15,7 @@
     void answerPositon(int16_t *position);
     void answerPositonMillimeter(float *position);
     void integralPosition(char *str);
+    void userOriginPositionInitial(void);
 
     uint8_t xyOrigin(void);
     uint8_t zOrigin(void);
@@ -26,21 +27,33 @@
     int putc(int c);
     uint8_t initial(void);
     uint8_t motorOff(void);
+    uint8_t motorOn(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);
+    void offsetXAxisAdjustment(int16_t axisData);
+    void offsetYAxisAdjustment(int16_t axisData);
+    void offsetZAxisAdjustment(int16_t axisData);
+
+    int32_t motorStateCheck(void);
+
 
 private:
 
     BufferedSerial _serial; // tx, rx(NC)
     DigitalIn _cts;
 
+    void translationToControlerAxisMoveDataFromRMLAxisMoveData(char *str);
+
+
     char B_masterTx[0xff];
-    double D_position[3]; //[0]:x, [1]:y, [2]:z
-    double D_userOriginPosition[3]; //[0]:x, [1]:y, [2]:z
+    int16_t D_position[3]; //[0]:x, [1]:y, [2]:z
+    int16_t D_userOriginPosition[3]; //[0]:x, [1]:y, [2]:z
+    int32_t motorState; // true:ON false:OFF
 
 #define countToMillimeter (0.025)
 #define Z_x (0)