MDX-15,20の制御用library
Diff: MDX20.cpp
- Revision:
- 5:bd414d8e483f
- Parent:
- 4:b01a67ab40cf
- Child:
- 6:b61356230acf
--- 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;