MDX20.cpp

00001 #include "MDX20.h"
00002 #include "BufferedSerial.h"
00003 
00004 //#define DEBUG
00005 #ifdef DEBUG
00006 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
00007 #else
00008 #define DEBUG_PRINT(...)
00009 #endif // DEBUG
00010 
00011 
00012 LocalFileSystem local("local");
00013 
00014 MDX20::MDX20(PinName tx, PinName rx, PinName cts) : _serial(tx, rx), _cts(cts, PullUp)
00015 {
00016 
00017     _serial.baud(9600);
00018     _serial.format(8,Serial::None,1);
00019 
00020 
00021 
00022 }
00023 
00024 MDX20::~MDX20()
00025 {
00026 }
00027 
00028 void MDX20::clearPositon(void)
00029 {
00030     D_position[Z_x] = 0;
00031     D_position[Z_y] = 0;
00032     D_position[Z_z] = 0;
00033 }
00034 
00035 void MDX20::answerPositon(int16_t *position)
00036 {
00037     *(position + Z_x) = D_position[Z_x];
00038     *(position + Z_y) = D_position[Z_y];
00039     *(position + Z_z) = D_position[Z_z];
00040 
00041 }
00042 
00043 void MDX20::answerPositonMillimeter(float *position)
00044 {
00045     *(position + Z_x) = (float)D_position[Z_x] * countToMillimeter;
00046     *(position + Z_y) = (float)D_position[Z_y] * countToMillimeter;
00047     *(position + Z_z) = (float)D_position[Z_z] * countToMillimeter;
00048 
00049 }
00050 
00051 
00052 void MDX20::integralPosition(char *str)
00053 {
00054     char strData[100];
00055     char *p;
00056     static uint8_t AorR = 'A';  // 'A'=absolute 'R'=relative
00057 
00058     strcpy(strData, str);
00059 
00060     if( 0 == strncmp("^PA", strData, 3)) {
00061         AorR = 'A';
00062         return;
00063     } else if( 0 == strncmp("^PR", strData, 3)) {
00064         AorR = 'R';
00065         *(str + 2) = 'A';  // MDX-20に送信するときは相対値から絶対値にする(この関数の中で相対値から絶対値にしているため）
00066         return;
00067     }
00068 
00069     // コマンド中の","を" "に置き換える
00070     while ((p = strchr(strData, ','))!=NULL) *p = ' ';
00071 
00072     int16_t a[3] = {0,0,0};
00073 
00074     if(strncmp(str, "!ZZ",3) == 0) {
00075         sscanf((strData + 3), "%d %d %d", &a[0], &a[1], &a[2]);
00076     } else if(strncmp(str, "Z",1) == 0) {
00077         sscanf((strData + 1), "%d %d %d", &a[0], &a[1], &a[2]);
00078     } else if(strncmp(str, "^PU",3) == 0) {
00079         sscanf((strData + 3), "%d %d", &a[0], &a[1]);
00080         a[2] = SHRT_MAX;
00081     } else if(strncmp(str, "^PD",3) == 0) {
00082         sscanf((strData + 3), "%d %d", &a[0], &a[1]);
00083         a[2] = SHRT_MAX;
00084     } else if(strncmp(str, "!ZM",3) == 0) {
00085         sscanf((strData + 3), "%d", &a[2]);
00086         a[0] = SHRT_MAX;
00087         a[1] = SHRT_MAX;
00088 
00089     } else {
00090         return;
00091     }
00092 
00093     if(AorR == 'A') {
00094         if(a[0] != SHRT_MAX) {
00095             D_position[Z_x] = a[0] + D_userOriginPosition[Z_x];
00096         }
00097         if(a[1] != SHRT_MAX) {
00098             D_position[Z_y] = a[1] + D_userOriginPosition[Z_y];
00099         }
00100         if(a[2] != SHRT_MAX) {
00101             D_position[Z_z] = a[2] + D_userOriginPosition[Z_z];
00102         }
00103     } else {
00104         // controler axis move data change to absolute from relative
00105         if(a[0] != SHRT_MAX) {
00106             D_position[Z_x] += a[0];
00107         }
00108         if(a[1] != SHRT_MAX) {
00109             D_position[Z_y] += a[1];
00110         }
00111         if(a[2] != SHRT_MAX) {
00112             D_position[Z_z] += a[2];
00113         }
00114     }
00115     translationToControlerAxisMoveDataFromRMLAxisMoveData(str);
00116 //    DEBUG_PRINT("x=%d y=%d z=%d \n", D_position[Z_x], D_position[Z_y], D_position[Z_z]);
00117 //    wait(0.1);
00118 }
00119 
00120 void MDX20::translationToControlerAxisMoveDataFromRMLAxisMoveData(char *str)
00121 {
00122     char buffer[50];
00123 
00124     if(strncmp(str, "!ZZ",3) == 0) {
00125         sprintf(buffer,"!ZZ%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
00126         strcpy(str, buffer);
00127     } else if(strncmp(str, "Z",1) == 0) {
00128         sprintf(buffer,"Z%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
00129         strcpy(str, buffer);
00130     } else if(strncmp(str, "^PU",3) == 0) {
00131         sprintf(buffer,"^PU%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
00132         strcpy(str, buffer);
00133     } else if(strncmp(str, "^PD",3) == 0) {
00134         sprintf(buffer,"^PD%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
00135         strcpy(str, buffer);
00136     } else if(strncmp(str, "!ZM",3) == 0) {
00137         sprintf(buffer,"!ZM%d;\r\n",D_position[Z_z]);
00138         strcpy(str, buffer);
00139     } else {
00140         // nothing
00141     }
00142 }
00143 
00144 
00145 uint8_t MDX20::xyOrigin(void)
00146 {
00147     char buffer[50];
00148     uint8_t ans;
00149     D_userOriginPosition[Z_x] = D_position[Z_x];
00150     D_userOriginPosition[Z_y] = D_position[Z_y];
00151 //    D_userOriginPosition[Z_z] = 0;
00152     FILE *fp;
00153 
00154     fp = fopen("/local/OriginX.ini", "w");
00155     sprintf(buffer,"%d",D_userOriginPosition[Z_x]);
00156     fprintf(fp, buffer);
00157     fclose(fp);
00158 
00159     fp = fopen("/local/OriginY.ini", "w");
00160     sprintf(buffer,"%d",D_userOriginPosition[Z_y]);
00161     fprintf(fp, buffer);
00162     fclose(fp);
00163 
00164     return ans;
00165 }
00166 
00167 uint8_t MDX20::zOrigin(void)
00168 {
00169     char buffer[50];
00170     uint8_t ans;
00171     D_userOriginPosition[Z_z] = D_position[Z_z];
00172 
00173     FILE *fp = fopen("/local/OriginZ.ini", "w");
00174     sprintf(buffer,"%d",D_userOriginPosition[Z_z]);
00175     fprintf(fp, buffer);
00176     fclose(fp);
00177 
00178     /* Z0 command use
00179         sprintf(buffer, "!ZO%d;",0);    // 今いる位置をZ原点にするので"0"を設定する
00180     //    printf("%s\r\n",buffer);
00181         ans &= sendData(buffer);
00182     */
00183     return ans;
00184 }
00185 
00186 /** offset加算後の各軸の可動範囲チェック
00187  * @para data RMLデータ一命令文字列の先頭アドレス
00188  * @returns
00189  *  false :逸脱あり
00190  *  true  :正常
00191  */
00192 int32_t MDX20::axisMovingCheck(char* data)
00193 {
00194     int32_t ans = true;
00195     integralPosition(data);
00196     if(
00197         ((D_position[Z_x] < Z_xAxisMin) || (Z_xAxisMax < D_position[Z_x])) ||
00198         ((D_position[Z_y] < Z_yAxisMin) || (Z_yAxisMax < D_position[Z_y])) ||
00199         ((D_position[Z_z] < Z_zAxisMin) || (Z_zAxisMax < D_position[Z_z]))
00200     ) {
00201         ans = false;
00202     }
00203     return ans;
00204 }
00205 
00206 /**
00207 * MDX-15/20へのデータ送信
00208 * @@para *data : データ一行の先頭アドレス
00209 * @@para uint8_t : 0:送信キャンセル 1:送信完了
00210 */
00211 uint8_t MDX20::sendData(char* data)
00212 {
00213     uint8_t ans = 0;    // 0:送信キャンセル 1:送信完了
00214     char buffer[50];
00215     strcpy(buffer, data);
00216 
00217     while(_cts != 0) {}
00218     wait(0.1);  // このwait timeがないとMDX-20からのwait指示を読み飛ばす
00219 
00220     if(strncmp(buffer, "!MC0", 4) == 0) {
00221         motorState = false;
00222     } else if(strncmp(buffer, "!MC1", 4) == 0) {
00223         motorState = true;
00224     } else {
00225         // nothing
00226     }
00227 
00228     DEBUG_PRINT("send = %s\r\n",buffer);
00229     integralPosition(buffer);
00230     _serial.printf("%s\r\n",buffer);
00231 
00232     // 可動範囲外に出たらmin,maxに差し替える
00233     if(D_position[Z_x] < Z_xAxisMin) {
00234         D_position[Z_x] = Z_xAxisMin;
00235     }
00236     if(Z_xAxisMax < D_position[Z_x]) {
00237         D_position[Z_x] = Z_xAxisMax;
00238     }
00239     if(D_position[Z_y] < Z_yAxisMin) {
00240         D_position[Z_y] = Z_yAxisMin;
00241     }
00242     if(Z_yAxisMax < D_position[Z_y]) {
00243         D_position[Z_y] = Z_yAxisMax;
00244     }
00245     if(D_position[Z_z] < Z_zAxisMin) {
00246         D_position[Z_z] = Z_zAxisMin;
00247     }
00248     if(Z_zAxisMax < D_position[Z_z]) {
00249         D_position[Z_z] = Z_zAxisMax;
00250     }
00251 
00252     ans = 1;
00253 
00254     return (ans);
00255 }
00256 
00257 /**
00258  *
00259  */
00260 uint8_t MDX20::reciveData(void)
00261 {
00262     char ans = 0;
00263     while(_serial.readable()) {
00264         ans = _serial.getc();
00265     }
00266     return ans;
00267 }
00268 
00269 int MDX20::putc(int c)
00270 {
00271     _serial.putc(c);
00272     return c;
00273 }
00274 
00275 uint8_t MDX20::initial(void)
00276 {
00277     uint8_t ans;
00278     char buffer[50];
00279     clearPositon();
00280     FILE *fp;
00281 
00282     fp = fopen("/local/ORIGINX.INI", "r");
00283     if ( fp != NULL ) {
00284         if(NULL != fgets(buffer, sizeof(buffer), fp)) {
00285             D_userOriginPosition[Z_x] = atoi(buffer);
00286         } else {
00287             // nothing
00288         }
00289         fclose(fp);
00290 
00291     } else {
00292         // nothing
00293     }
00294 
00295     fp = fopen("/local/ORIGINY.INI", "r");
00296     if ( fp != NULL ) {
00297         if(NULL != fgets(buffer, sizeof(buffer), fp)) {
00298             D_userOriginPosition[Z_y] = atoi(buffer);
00299         } else {
00300             // nothing
00301         }
00302         fclose(fp);
00303 
00304 
00305     } else {
00306         // nothing
00307     }
00308 
00309     fp = fopen("/local/ORIGINZ.INI", "r");
00310     if ( fp != NULL ) {
00311         if(NULL != fgets(buffer, sizeof(buffer), fp)) {
00312             D_userOriginPosition[Z_z] = atoi(buffer);
00313         } else {
00314             // nothing
00315         }
00316         fclose(fp);
00317 
00318     } else {
00319         // nothing
00320     }
00321 
00322     ans &= sendData("^IN;");
00323     ans &= sendData("!MC0;");
00324     ans &= sendData("!ZO0;");
00325     ans &= sendData("!ZO0;"); // Z axis origin initialaize
00326     ans &= sendData("^PA;");
00327 
00328     sprintf(buffer,"Z%d,%d,%d;",-D_userOriginPosition[Z_x], -D_userOriginPosition[Z_y], -D_userOriginPosition[Z_z]);
00329 
00330     ans &= sendData(buffer);
00331 
00332     return (ans);
00333 }
00334 
00335 uint8_t MDX20::motorOff(void)
00336 {
00337     uint8_t ans;
00338     ans &= sendData("!MC0;");
00339     return (ans);
00340 }
00341 
00342 /**
00343  * @note モータを動かすためには位置設定コマンドの発行が必要
00344  */
00345 uint8_t MDX20::motorOn(void)
00346 {
00347     uint8_t ans;
00348     ans &= sendData("!MC1;");
00349     ans &= sendData("^PR;");
00350     ans &= sendData("Z0,0,0");
00351     return (ans);
00352 }
00353 
00354 
00355 uint8_t MDX20::userOriginInitial(void)
00356 {
00357     char buffer[50];
00358     uint8_t ans;
00359 
00360     ans &= sendData("^PA;");
00361     ans &= sendData("!MC0;");
00362 
00363     sprintf(buffer, "Z%d,%d,%d;",0, 0, 0);
00364     ans &= sendData(buffer);
00365 
00366     return ans;
00367 }
00368 
00369 uint8_t MDX20::userXYOriginInitial(void)
00370 {
00371     char buffer[50];
00372     uint8_t ans;
00373 
00374     ans &= sendData("^PA;");
00375     ans &= sendData("!MC0;");
00376 
00377     sprintf(buffer, "Z%d,%d,%d;",0, 0, D_position[Z_z] - D_userOriginPosition[Z_z]);
00378     ans &= sendData(buffer);
00379 
00380     return ans;
00381 }
00382 
00383 uint8_t MDX20::userZOriginInitial(void)
00384 {
00385     char buffer[50];
00386     uint8_t ans;
00387 
00388     ans &= sendData("^PA;");
00389     ans &= sendData("!MC0;");
00390 
00391     sprintf(buffer, "!ZM%d;",0);
00392     ans &= sendData(buffer);
00393 
00394     return ans;
00395 }
00396 
00397 uint8_t MDX20::final(void)
00398 {
00399     uint8_t ans;
00400 
00401 // モータを止めて、Z軸を機械原点に戻す。(X,Y軸はその場にとどめる)
00402     ans &= sendData("!MC0;");
00403     ans &= sendData("^PA;");
00404     ans &= sendData("!ZO0;");
00405     ans &= sendData("!ZO0;"); // Z axis origin initialaize
00406 
00407 /* このコードだとZ軸が原点に戻らないままXY実を原点に戻すためワークに引っ掛かり破損する
00408     ans &= sendData("!MC0;");
00409     ans &= sendData("^PA;");
00410     ans &= sendData("Z0,0,0;");
00411 */
00412     clearPositon();
00413     ans &= sendData("^IN;");
00414     return (ans);
00415 }
00416 /*
00417 uint8_t MDX20::zeroSetting(void)
00418 {
00419     uint8_t ans;
00420 
00421     ans &= sendData("!X00");
00422     ans &= sendData("!Y00");
00423     ans &= sendData("!Z00");
00424     clearPositon();
00425     return (ans);
00426 }
00427 */
00428 
00429 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
00430 {
00431     uint8_t ans;
00432     char buffer[50];
00433 
00434     ans &= sendData("!MC0;");
00435     strcpy(buffer, "^PR;");
00436     ans &= sendData(buffer);
00437     sprintf(buffer, "Z%05d,%05d,%05d;",x, y, z);
00438     ans &= sendData(buffer);
00439 
00440     return (ans);
00441 }
00442 
00443 void MDX20::offsetXAxisAdjustment(int16_t axisData)
00444 {
00445     XYZMove(axisData, 0, 0);
00446 //    D_userOriginPosition[Z_x] = 0;
00447 
00448 }
00449 
00450 void MDX20::offsetYAxisAdjustment(int16_t axisData)
00451 {
00452     XYZMove(0, axisData, 0);
00453 //    D_userOriginPosition[Z_y] = 0;
00454 
00455 }
00456 
00457 void MDX20::offsetZAxisAdjustment(int16_t axisData)
00458 {
00459     XYZMove(0, 0, axisData);
00460 //    D_userOriginPosition[Z_z] = 0;
00461 
00462 }
00463 
00464 void MDX20::userOriginPositionInitial(void)
00465 {
00466     D_userOriginPosition[Z_x] = 0;
00467     D_userOriginPosition[Z_y] = 0;
00468     D_userOriginPosition[Z_z] = 0;
00469 }
00470 
00471 int32_t MDX20::motorStateCheck(void)
00472 {
00473     return motorState;
00474 }
00475 
00476 /** MDX-20の接続確認
00477  * @return true:接続あり false:なし
00478  */
00479 int32_t MDX20::connectCheck(void)
00480 {
00481    int32_t ans = false;
00482    
00483    if(_cts == 0){
00484        ans = true;
00485        }else{
00486            ans = false;
00487         }
00488     return (ans);
00489     }
00490