suu pen / MDX20

MDX-15,20の制御用library

MDX20.cpp@9:5ee05e9c5aca, 2018-10-14 (annotated)

Committer:
suupen
Date:
Sun Oct 14 00:12:37 2018 +0000
Revision:
9:5ee05e9c5aca
Parent:
8:e65e532d1933 
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
suupen 1:1751fee99a0d 1 #include "MDX20.h"
suupen 1:1751fee99a0d 2 #include "BufferedSerial.h"
suupen 1:1751fee99a0d 3
suupen 8:e65e532d1933 4 //#define DEBUG
suupen 6:b61356230acf 5 #ifdef DEBUG
suupen 6:b61356230acf 6 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
suupen 6:b61356230acf 7 #else
suupen 6:b61356230acf 8 #define DEBUG_PRINT(...)
suupen 6:b61356230acf 9 #endif // DEBUG
suupen 6:b61356230acf 10
suupen 6:b61356230acf 11
suupen 6:b61356230acf 12 LocalFileSystem local("local");
suupen 1:1751fee99a0d 13
suupen 1:1751fee99a0d 14 MDX20::MDX20(PinName tx, PinName rx, PinName cts) : _serial(tx, rx), _cts(cts, PullUp)
suupen 1:1751fee99a0d 15 {
suupen 1:1751fee99a0d 16
suupen 1:1751fee99a0d 17 _serial.baud(9600);
suupen 1:1751fee99a0d 18 _serial.format(8,Serial::None,1);
suupen 6:b61356230acf 19
suupen 6:b61356230acf 20
suupen 6:b61356230acf 21
suupen 1:1751fee99a0d 22 }
suupen 1:1751fee99a0d 23
suupen 1:1751fee99a0d 24 MDX20::~MDX20()
suupen 1:1751fee99a0d 25 {
suupen 1:1751fee99a0d 26 }
suupen 1:1751fee99a0d 27
suupen 2:8446eb1774c4 28 void MDX20::clearPositon(void)
suupen 2:8446eb1774c4 29 {
suupen 2:8446eb1774c4 30 D_position[Z_x] = 0;
suupen 2:8446eb1774c4 31 D_position[Z_y] = 0;
suupen 2:8446eb1774c4 32 D_position[Z_z] = 0;
suupen 2:8446eb1774c4 33 }
suupen 3:3bf2936b8d9b 34
suupen 2:8446eb1774c4 35 void MDX20::answerPositon(int16_t *position)
suupen 2:8446eb1774c4 36 {
suupen 2:8446eb1774c4 37 *(position + Z_x) = D_position[Z_x];
suupen 2:8446eb1774c4 38 *(position + Z_y) = D_position[Z_y];
suupen 2:8446eb1774c4 39 *(position + Z_z) = D_position[Z_z];
suupen 2:8446eb1774c4 40
suupen 3:3bf2936b8d9b 41 }
suupen 3:3bf2936b8d9b 42
suupen 3:3bf2936b8d9b 43 void MDX20::answerPositonMillimeter(float *position)
suupen 3:3bf2936b8d9b 44 {
suupen 3:3bf2936b8d9b 45 *(position + Z_x) = (float)D_position[Z_x] * countToMillimeter;
suupen 3:3bf2936b8d9b 46 *(position + Z_y) = (float)D_position[Z_y] * countToMillimeter;
suupen 3:3bf2936b8d9b 47 *(position + Z_z) = (float)D_position[Z_z] * countToMillimeter;
suupen 3:3bf2936b8d9b 48
suupen 3:3bf2936b8d9b 49 }
suupen 3:3bf2936b8d9b 50
suupen 3:3bf2936b8d9b 51
suupen 3:3bf2936b8d9b 52 void MDX20::integralPosition(char *str)
suupen 3:3bf2936b8d9b 53 {
suupen 3:3bf2936b8d9b 54 char strData[100];
suupen 3:3bf2936b8d9b 55 char *p;
suupen 3:3bf2936b8d9b 56 static uint8_t AorR = 'A'; // 'A'=absolute 'R'=relative
suupen 3:3bf2936b8d9b 57
suupen 3:3bf2936b8d9b 58 strcpy(strData, str);
suupen 3:3bf2936b8d9b 59
suupen 3:3bf2936b8d9b 60 if( 0 == strncmp("^PA", strData, 3)) {
suupen 3:3bf2936b8d9b 61 AorR = 'A';
suupen 5:bd414d8e483f 62 return;
suupen 3:3bf2936b8d9b 63 } else if( 0 == strncmp("^PR", strData, 3)) {
suupen 3:3bf2936b8d9b 64 AorR = 'R';
suupen 6:b61356230acf 65 *(str + 2) = 'A'; // MDX-20に送信するときは相対値から絶対値にする(この関数の中で相対値から絶対値にしているため）
suupen 5:bd414d8e483f 66 return;
suupen 5:bd414d8e483f 67 }
suupen 5:bd414d8e483f 68
suupen 5:bd414d8e483f 69 // コマンド中の","を" "に置き換える
suupen 5:bd414d8e483f 70 while ((p = strchr(strData, ','))!=NULL) *p = ' ';
suupen 5:bd414d8e483f 71
suupen 6:b61356230acf 72 int16_t a[3] = {0,0,0};
suupen 5:bd414d8e483f 73
suupen 5:bd414d8e483f 74 if(strncmp(str, "!ZZ",3) == 0) {
suupen 6:b61356230acf 75 sscanf((strData + 3), "%d %d %d", &a[0], &a[1], &a[2]);
suupen 5:bd414d8e483f 76 } else if(strncmp(str, "Z",1) == 0) {
suupen 6:b61356230acf 77 sscanf((strData + 1), "%d %d %d", &a[0], &a[1], &a[2]);
suupen 5:bd414d8e483f 78 } else if(strncmp(str, "^PU",3) == 0) {
suupen 6:b61356230acf 79 sscanf((strData + 3), "%d %d", &a[0], &a[1]);
suupen 6:b61356230acf 80 a[2] = SHRT_MAX;
suupen 5:bd414d8e483f 81 } else if(strncmp(str, "^PD",3) == 0) {
suupen 6:b61356230acf 82 sscanf((strData + 3), "%d %d", &a[0], &a[1]);
suupen 6:b61356230acf 83 a[2] = SHRT_MAX;
suupen 6:b61356230acf 84 } else if(strncmp(str, "!ZM",3) == 0) {
suupen 6:b61356230acf 85 sscanf((strData + 3), "%d", &a[2]);
suupen 6:b61356230acf 86 a[0] = SHRT_MAX;
suupen 6:b61356230acf 87 a[1] = SHRT_MAX;
suupen 6:b61356230acf 88
suupen 6:b61356230acf 89 } else {
suupen 6:b61356230acf 90 return;
suupen 3:3bf2936b8d9b 91 }
suupen 3:3bf2936b8d9b 92
suupen 3:3bf2936b8d9b 93 if(AorR == 'A') {
suupen 6:b61356230acf 94 if(a[0] != SHRT_MAX) {
suupen 6:b61356230acf 95 D_position[Z_x] = a[0] + D_userOriginPosition[Z_x];
suupen 6:b61356230acf 96 }
suupen 6:b61356230acf 97 if(a[1] != SHRT_MAX) {
suupen 6:b61356230acf 98 D_position[Z_y] = a[1] + D_userOriginPosition[Z_y];
suupen 6:b61356230acf 99 }
suupen 6:b61356230acf 100 if(a[2] != SHRT_MAX) {
suupen 6:b61356230acf 101 D_position[Z_z] = a[2] + D_userOriginPosition[Z_z];
suupen 5:bd414d8e483f 102 }
suupen 3:3bf2936b8d9b 103 } else {
suupen 6:b61356230acf 104 // controler axis move data change to absolute from relative
suupen 6:b61356230acf 105 if(a[0] != SHRT_MAX) {
suupen 6:b61356230acf 106 D_position[Z_x] += a[0];
suupen 6:b61356230acf 107 }
suupen 6:b61356230acf 108 if(a[1] != SHRT_MAX) {
suupen 6:b61356230acf 109 D_position[Z_y] += a[1];
suupen 6:b61356230acf 110 }
suupen 6:b61356230acf 111 if(a[2] != SHRT_MAX) {
suupen 6:b61356230acf 112 D_position[Z_z] += a[2];
suupen 5:bd414d8e483f 113 }
suupen 3:3bf2936b8d9b 114 }
suupen 6:b61356230acf 115 translationToControlerAxisMoveDataFromRMLAxisMoveData(str);
suupen 6:b61356230acf 116 // DEBUG_PRINT("x=%d y=%d z=%d \n", D_position[Z_x], D_position[Z_y], D_position[Z_z]);
suupen 8:e65e532d1933 117 // wait(0.1);
suupen 3:3bf2936b8d9b 118 }
suupen 3:3bf2936b8d9b 119
suupen 6:b61356230acf 120 void MDX20::translationToControlerAxisMoveDataFromRMLAxisMoveData(char *str)
suupen 6:b61356230acf 121 {
suupen 6:b61356230acf 122 char buffer[50];
suupen 6:b61356230acf 123
suupen 6:b61356230acf 124 if(strncmp(str, "!ZZ",3) == 0) {
suupen 6:b61356230acf 125 sprintf(buffer,"!ZZ%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
suupen 6:b61356230acf 126 strcpy(str, buffer);
suupen 6:b61356230acf 127 } else if(strncmp(str, "Z",1) == 0) {
suupen 6:b61356230acf 128 sprintf(buffer,"Z%d,%d,%d;\r\n",D_position[Z_x], D_position[Z_y], D_position[Z_z]);
suupen 6:b61356230acf 129 strcpy(str, buffer);
suupen 6:b61356230acf 130 } else if(strncmp(str, "^PU",3) == 0) {
suupen 6:b61356230acf 131 sprintf(buffer,"^PU%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
suupen 6:b61356230acf 132 strcpy(str, buffer);
suupen 6:b61356230acf 133 } else if(strncmp(str, "^PD",3) == 0) {
suupen 6:b61356230acf 134 sprintf(buffer,"^PD%d,%d;\r\n",D_position[Z_x], D_position[Z_y]);
suupen 6:b61356230acf 135 strcpy(str, buffer);
suupen 6:b61356230acf 136 } else if(strncmp(str, "!ZM",3) == 0) {
suupen 6:b61356230acf 137 sprintf(buffer,"!ZM%d;\r\n",D_position[Z_z]);
suupen 6:b61356230acf 138 strcpy(str, buffer);
suupen 6:b61356230acf 139 } else {
suupen 6:b61356230acf 140 // nothing
suupen 6:b61356230acf 141 }
suupen 6:b61356230acf 142 }
suupen 6:b61356230acf 143
suupen 6:b61356230acf 144
suupen 3:3bf2936b8d9b 145 uint8_t MDX20::xyOrigin(void)
suupen 2:8446eb1774c4 146 {
suupen 6:b61356230acf 147 char buffer[50];
suupen 3:3bf2936b8d9b 148 uint8_t ans;
suupen 3:3bf2936b8d9b 149 D_userOriginPosition[Z_x] = D_position[Z_x];
suupen 3:3bf2936b8d9b 150 D_userOriginPosition[Z_y] = D_position[Z_y];
suupen 5:bd414d8e483f 151 // D_userOriginPosition[Z_z] = 0;
suupen 6:b61356230acf 152 FILE *fp;
suupen 3:3bf2936b8d9b 153
suupen 6:b61356230acf 154 fp = fopen("/local/OriginX.ini", "w");
suupen 6:b61356230acf 155 sprintf(buffer,"%d",D_userOriginPosition[Z_x]);
suupen 6:b61356230acf 156 fprintf(fp, buffer);
suupen 6:b61356230acf 157 fclose(fp);
suupen 6:b61356230acf 158
suupen 6:b61356230acf 159 fp = fopen("/local/OriginY.ini", "w");
suupen 6:b61356230acf 160 sprintf(buffer,"%d",D_userOriginPosition[Z_y]);
suupen 6:b61356230acf 161 fprintf(fp, buffer);
suupen 6:b61356230acf 162 fclose(fp);
suupen 3:3bf2936b8d9b 163
suupen 3:3bf2936b8d9b 164 return ans;
suupen 3:3bf2936b8d9b 165 }
suupen 3:3bf2936b8d9b 166
suupen 3:3bf2936b8d9b 167 uint8_t MDX20::zOrigin(void)
suupen 3:3bf2936b8d9b 168 {
suupen 6:b61356230acf 169 char buffer[50];
suupen 3:3bf2936b8d9b 170 uint8_t ans;
suupen 5:bd414d8e483f 171 D_userOriginPosition[Z_z] = D_position[Z_z];
suupen 6:b61356230acf 172
suupen 6:b61356230acf 173 FILE *fp = fopen("/local/OriginZ.ini", "w");
suupen 6:b61356230acf 174 sprintf(buffer,"%d",D_userOriginPosition[Z_z]);
suupen 6:b61356230acf 175 fprintf(fp, buffer);
suupen 6:b61356230acf 176 fclose(fp);
suupen 3:3bf2936b8d9b 177
suupen 5:bd414d8e483f 178 /* Z0 command use
suupen 5:bd414d8e483f 179 sprintf(buffer, "!ZO%d;",0); // 今いる位置をZ原点にするので"0"を設定する
suupen 5:bd414d8e483f 180 // printf("%s\r\n",buffer);
suupen 5:bd414d8e483f 181 ans &= sendData(buffer);
suupen 5:bd414d8e483f 182 */
suupen 3:3bf2936b8d9b 183 return ans;
suupen 3:3bf2936b8d9b 184 }
suupen 2:8446eb1774c4 185
suupen 8:e65e532d1933 186 /** offset加算後の各軸の可動範囲チェック
suupen 8:e65e532d1933 187 * @para data RMLデータ一命令文字列の先頭アドレス
suupen 8:e65e532d1933 188 * @returns
suupen 8:e65e532d1933 189 * false :逸脱あり
suupen 8:e65e532d1933 190 * true :正常
suupen 8:e65e532d1933 191 */
suupen 8:e65e532d1933 192 int32_t MDX20::axisMovingCheck(char* data)
suupen 8:e65e532d1933 193 {
suupen 8:e65e532d1933 194 int32_t ans = true;
suupen 8:e65e532d1933 195 integralPosition(data);
suupen 8:e65e532d1933 196 if(
suupen 8:e65e532d1933 197 ((D_position[Z_x] < Z_xAxisMin) || (Z_xAxisMax < D_position[Z_x])) ||
suupen 8:e65e532d1933 198 ((D_position[Z_y] < Z_yAxisMin) || (Z_yAxisMax < D_position[Z_y])) ||
suupen 8:e65e532d1933 199 ((D_position[Z_z] < Z_zAxisMin) || (Z_zAxisMax < D_position[Z_z]))
suupen 8:e65e532d1933 200 ) {
suupen 8:e65e532d1933 201 ans = false;
suupen 8:e65e532d1933 202 }
suupen 8:e65e532d1933 203 return ans;
suupen 8:e65e532d1933 204 }
suupen 8:e65e532d1933 205
suupen 1:1751fee99a0d 206 /**
suupen 1:1751fee99a0d 207 * MDX-15/20へのデータ送信
suupen 1:1751fee99a0d 208 * @@para *data : データ一行の先頭アドレス
suupen 1:1751fee99a0d 209 * @@para uint8_t : 0:送信キャンセル 1:送信完了
suupen 1:1751fee99a0d 210 */
suupen 1:1751fee99a0d 211 uint8_t MDX20::sendData(char* data)
suupen 1:1751fee99a0d 212 {
suupen 1:1751fee99a0d 213 uint8_t ans = 0; // 0:送信キャンセル 1:送信完了
suupen 6:b61356230acf 214 char buffer[50];
suupen 6:b61356230acf 215 strcpy(buffer, data);
suupen 1:1751fee99a0d 216
suupen 2:8446eb1774c4 217 while(_cts != 0) {}
suupen 2:8446eb1774c4 218 wait(0.1); // このwait timeがないとMDX-20からのwait指示を読み飛ばす
suupen 3:3bf2936b8d9b 219
suupen 6:b61356230acf 220 if(strncmp(buffer, "!MC0", 4) == 0) {
suupen 6:b61356230acf 221 motorState = false;
suupen 6:b61356230acf 222 } else if(strncmp(buffer, "!MC1", 4) == 0) {
suupen 6:b61356230acf 223 motorState = true;
suupen 6:b61356230acf 224 } else {
suupen 6:b61356230acf 225 // nothing
suupen 6:b61356230acf 226 }
suupen 6:b61356230acf 227
suupen 6:b61356230acf 228 DEBUG_PRINT("send = %s\r\n",buffer);
suupen 6:b61356230acf 229 integralPosition(buffer);
suupen 6:b61356230acf 230 _serial.printf("%s\r\n",buffer);
suupen 6:b61356230acf 231
suupen 8:e65e532d1933 232 // 可動範囲外に出たらmin,maxに差し替える
suupen 8:e65e532d1933 233 if(D_position[Z_x] < Z_xAxisMin) {
suupen 8:e65e532d1933 234 D_position[Z_x] = Z_xAxisMin;
suupen 8:e65e532d1933 235 }
suupen 8:e65e532d1933 236 if(Z_xAxisMax < D_position[Z_x]) {
suupen 8:e65e532d1933 237 D_position[Z_x] = Z_xAxisMax;
suupen 8:e65e532d1933 238 }
suupen 8:e65e532d1933 239 if(D_position[Z_y] < Z_yAxisMin) {
suupen 8:e65e532d1933 240 D_position[Z_y] = Z_yAxisMin;
suupen 8:e65e532d1933 241 }
suupen 8:e65e532d1933 242 if(Z_yAxisMax < D_position[Z_y]) {
suupen 8:e65e532d1933 243 D_position[Z_y] = Z_yAxisMax;
suupen 8:e65e532d1933 244 }
suupen 8:e65e532d1933 245 if(D_position[Z_z] < Z_zAxisMin) {
suupen 8:e65e532d1933 246 D_position[Z_z] = Z_zAxisMin;
suupen 8:e65e532d1933 247 }
suupen 8:e65e532d1933 248 if(Z_zAxisMax < D_position[Z_z]) {
suupen 8:e65e532d1933 249 D_position[Z_z] = Z_zAxisMax;
suupen 8:e65e532d1933 250 }
suupen 8:e65e532d1933 251
suupen 2:8446eb1774c4 252 ans = 1;
suupen 2:8446eb1774c4 253
suupen 1:1751fee99a0d 254 return (ans);
suupen 1:1751fee99a0d 255 }
suupen 1:1751fee99a0d 256
suupen 9:5ee05e9c5aca 257 /**
suupen 9:5ee05e9c5aca 258 *
suupen 9:5ee05e9c5aca 259 */
suupen 3:3bf2936b8d9b 260 uint8_t MDX20::reciveData(void)
suupen 3:3bf2936b8d9b 261 {
suupen 3:3bf2936b8d9b 262 char ans = 0;
suupen 3:3bf2936b8d9b 263 while(_serial.readable()) {
suupen 3:3bf2936b8d9b 264 ans = _serial.getc();
suupen 3:3bf2936b8d9b 265 }
suupen 3:3bf2936b8d9b 266 return ans;
suupen 3:3bf2936b8d9b 267 }
suupen 3:3bf2936b8d9b 268
suupen 1:1751fee99a0d 269 int MDX20::putc(int c)
suupen 1:1751fee99a0d 270 {
suupen 1:1751fee99a0d 271 _serial.putc(c);
suupen 1:1751fee99a0d 272 return c;
suupen 1:1751fee99a0d 273 }
suupen 1:1751fee99a0d 274
suupen 2:8446eb1774c4 275 uint8_t MDX20::initial(void)
suupen 2:8446eb1774c4 276 {
suupen 2:8446eb1774c4 277 uint8_t ans;
suupen 6:b61356230acf 278 char buffer[50];
suupen 2:8446eb1774c4 279 clearPositon();
suupen 6:b61356230acf 280 FILE *fp;
suupen 6:b61356230acf 281
suupen 6:b61356230acf 282 fp = fopen("/local/ORIGINX.INI", "r");
suupen 6:b61356230acf 283 if ( fp != NULL ) {
suupen 6:b61356230acf 284 if(NULL != fgets(buffer, sizeof(buffer), fp)) {
suupen 6:b61356230acf 285 D_userOriginPosition[Z_x] = atoi(buffer);
suupen 6:b61356230acf 286 } else {
suupen 6:b61356230acf 287 // nothing
suupen 6:b61356230acf 288 }
suupen 6:b61356230acf 289 fclose(fp);
suupen 6:b61356230acf 290
suupen 6:b61356230acf 291 } else {
suupen 6:b61356230acf 292 // nothing
suupen 6:b61356230acf 293 }
suupen 6:b61356230acf 294
suupen 6:b61356230acf 295 fp = fopen("/local/ORIGINY.INI", "r");
suupen 6:b61356230acf 296 if ( fp != NULL ) {
suupen 6:b61356230acf 297 if(NULL != fgets(buffer, sizeof(buffer), fp)) {
suupen 6:b61356230acf 298 D_userOriginPosition[Z_y] = atoi(buffer);
suupen 6:b61356230acf 299 } else {
suupen 6:b61356230acf 300 // nothing
suupen 6:b61356230acf 301 }
suupen 6:b61356230acf 302 fclose(fp);
suupen 6:b61356230acf 303
suupen 6:b61356230acf 304
suupen 6:b61356230acf 305 } else {
suupen 6:b61356230acf 306 // nothing
suupen 6:b61356230acf 307 }
suupen 6:b61356230acf 308
suupen 6:b61356230acf 309 fp = fopen("/local/ORIGINZ.INI", "r");
suupen 6:b61356230acf 310 if ( fp != NULL ) {
suupen 6:b61356230acf 311 if(NULL != fgets(buffer, sizeof(buffer), fp)) {
suupen 6:b61356230acf 312 D_userOriginPosition[Z_z] = atoi(buffer);
suupen 6:b61356230acf 313 } else {
suupen 6:b61356230acf 314 // nothing
suupen 6:b61356230acf 315 }
suupen 6:b61356230acf 316 fclose(fp);
suupen 6:b61356230acf 317
suupen 6:b61356230acf 318 } else {
suupen 6:b61356230acf 319 // nothing
suupen 6:b61356230acf 320 }
suupen 6:b61356230acf 321
suupen 6:b61356230acf 322 ans &= sendData("^IN;");
suupen 6:b61356230acf 323 ans &= sendData("!MC0;");
suupen 6:b61356230acf 324 ans &= sendData("!ZO0;");
suupen 6:b61356230acf 325 ans &= sendData("!ZO0;"); // Z axis origin initialaize
suupen 6:b61356230acf 326 ans &= sendData("^PA;");
suupen 6:b61356230acf 327
suupen 6:b61356230acf 328 sprintf(buffer,"Z%d,%d,%d;",-D_userOriginPosition[Z_x], -D_userOriginPosition[Z_y], -D_userOriginPosition[Z_z]);
suupen 6:b61356230acf 329
suupen 6:b61356230acf 330 ans &= sendData(buffer);
suupen 6:b61356230acf 331
suupen 2:8446eb1774c4 332 return (ans);
suupen 2:8446eb1774c4 333 }
suupen 2:8446eb1774c4 334
suupen 4:b01a67ab40cf 335 uint8_t MDX20::motorOff(void)
suupen 4:b01a67ab40cf 336 {
suupen 4:b01a67ab40cf 337 uint8_t ans;
suupen 6:b61356230acf 338 ans &= sendData("!MC0;");
suupen 4:b01a67ab40cf 339 return (ans);
suupen 4:b01a67ab40cf 340 }
suupen 4:b01a67ab40cf 341
suupen 9:5ee05e9c5aca 342 /**
suupen 9:5ee05e9c5aca 343 * @note モータを動かすためには位置設定コマンドの発行が必要
suupen 9:5ee05e9c5aca 344 */
suupen 6:b61356230acf 345 uint8_t MDX20::motorOn(void)
suupen 6:b61356230acf 346 {
suupen 6:b61356230acf 347 uint8_t ans;
suupen 8:e65e532d1933 348 ans &= sendData("!MC1;");
suupen 8:e65e532d1933 349 ans &= sendData("^PR;");
suupen 8:e65e532d1933 350 ans &= sendData("Z0,0,0");
suupen 6:b61356230acf 351 return (ans);
suupen 6:b61356230acf 352 }
suupen 6:b61356230acf 353
suupen 6:b61356230acf 354
suupen 3:3bf2936b8d9b 355 uint8_t MDX20::userOriginInitial(void)
suupen 3:3bf2936b8d9b 356 {
suupen 6:b61356230acf 357 char buffer[50];
suupen 3:3bf2936b8d9b 358 uint8_t ans;
suupen 3:3bf2936b8d9b 359
suupen 6:b61356230acf 360 ans &= sendData("^PA;");
suupen 6:b61356230acf 361 ans &= sendData("!MC0;");
suupen 3:3bf2936b8d9b 362
suupen 6:b61356230acf 363 sprintf(buffer, "Z%d,%d,%d;",0, 0, 0);
suupen 3:3bf2936b8d9b 364 ans &= sendData(buffer);
suupen 3:3bf2936b8d9b 365
suupen 3:3bf2936b8d9b 366 return ans;
suupen 3:3bf2936b8d9b 367 }
suupen 3:3bf2936b8d9b 368
suupen 9:5ee05e9c5aca 369 uint8_t MDX20::userXYOriginInitial(void)
suupen 9:5ee05e9c5aca 370 {
suupen 9:5ee05e9c5aca 371 char buffer[50];
suupen 9:5ee05e9c5aca 372 uint8_t ans;
suupen 9:5ee05e9c5aca 373
suupen 9:5ee05e9c5aca 374 ans &= sendData("^PA;");
suupen 9:5ee05e9c5aca 375 ans &= sendData("!MC0;");
suupen 9:5ee05e9c5aca 376
suupen 9:5ee05e9c5aca 377 sprintf(buffer, "Z%d,%d,%d;",0, 0, D_position[Z_z] - D_userOriginPosition[Z_z]);
suupen 9:5ee05e9c5aca 378 ans &= sendData(buffer);
suupen 9:5ee05e9c5aca 379
suupen 9:5ee05e9c5aca 380 return ans;
suupen 9:5ee05e9c5aca 381 }
suupen 9:5ee05e9c5aca 382
suupen 9:5ee05e9c5aca 383 uint8_t MDX20::userZOriginInitial(void)
suupen 9:5ee05e9c5aca 384 {
suupen 9:5ee05e9c5aca 385 char buffer[50];
suupen 9:5ee05e9c5aca 386 uint8_t ans;
suupen 9:5ee05e9c5aca 387
suupen 9:5ee05e9c5aca 388 ans &= sendData("^PA;");
suupen 9:5ee05e9c5aca 389 ans &= sendData("!MC0;");
suupen 9:5ee05e9c5aca 390
suupen 9:5ee05e9c5aca 391 sprintf(buffer, "!ZM%d;",0);
suupen 9:5ee05e9c5aca 392 ans &= sendData(buffer);
suupen 9:5ee05e9c5aca 393
suupen 9:5ee05e9c5aca 394 return ans;
suupen 9:5ee05e9c5aca 395 }
suupen 9:5ee05e9c5aca 396
suupen 2:8446eb1774c4 397 uint8_t MDX20::final(void)
suupen 2:8446eb1774c4 398 {
suupen 3:3bf2936b8d9b 399 uint8_t ans;
suupen 9:5ee05e9c5aca 400
suupen 9:5ee05e9c5aca 401 // モータを止めて、Z軸を機械原点に戻す。(X,Y軸はその場にとどめる)
suupen 9:5ee05e9c5aca 402 ans &= sendData("!MC0;");
suupen 9:5ee05e9c5aca 403 ans &= sendData("^PA;");
suupen 9:5ee05e9c5aca 404 ans &= sendData("!ZO0;");
suupen 9:5ee05e9c5aca 405 ans &= sendData("!ZO0;"); // Z axis origin initialaize
suupen 9:5ee05e9c5aca 406
suupen 9:5ee05e9c5aca 407 /* このコードだとZ軸が原点に戻らないままXY実を原点に戻すためワークに引っ掛かり破損する
suupen 6:b61356230acf 408 ans &= sendData("!MC0;");
suupen 6:b61356230acf 409 ans &= sendData("^PA;");
suupen 6:b61356230acf 410 ans &= sendData("Z0,0,0;");
suupen 9:5ee05e9c5aca 411 */
suupen 2:8446eb1774c4 412 clearPositon();
suupen 6:b61356230acf 413 ans &= sendData("^IN;");
suupen 2:8446eb1774c4 414 return (ans);
suupen 3:3bf2936b8d9b 415 }
suupen 5:bd414d8e483f 416 /*
suupen 2:8446eb1774c4 417 uint8_t MDX20::zeroSetting(void)
suupen 2:8446eb1774c4 418 {
suupen 2:8446eb1774c4 419 uint8_t ans;
suupen 2:8446eb1774c4 420
suupen 2:8446eb1774c4 421 ans &= sendData("!X00");
suupen 2:8446eb1774c4 422 ans &= sendData("!Y00");
suupen 2:8446eb1774c4 423 ans &= sendData("!Z00");
suupen 2:8446eb1774c4 424 clearPositon();
suupen 2:8446eb1774c4 425 return (ans);
suupen 2:8446eb1774c4 426 }
suupen 5:bd414d8e483f 427 */
suupen 5:bd414d8e483f 428
suupen 2:8446eb1774c4 429 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
suupen 2:8446eb1774c4 430 {
suupen 2:8446eb1774c4 431 uint8_t ans;
suupen 6:b61356230acf 432 char buffer[50];
suupen 2:8446eb1774c4 433
suupen 6:b61356230acf 434 ans &= sendData("!MC0;");
suupen 6:b61356230acf 435 strcpy(buffer, "^PR;");
suupen 6:b61356230acf 436 ans &= sendData(buffer);
suupen 6:b61356230acf 437 sprintf(buffer, "Z%05d,%05d,%05d;",x, y, z);
suupen 2:8446eb1774c4 438 ans &= sendData(buffer);
suupen 2:8446eb1774c4 439
suupen 2:8446eb1774c4 440 return (ans);
suupen 2:8446eb1774c4 441 }
suupen 6:b61356230acf 442
suupen 6:b61356230acf 443 void MDX20::offsetXAxisAdjustment(int16_t axisData)
suupen 6:b61356230acf 444 {
suupen 6:b61356230acf 445 XYZMove(axisData, 0, 0);
suupen 6:b61356230acf 446 // D_userOriginPosition[Z_x] = 0;
suupen 6:b61356230acf 447
suupen 6:b61356230acf 448 }
suupen 6:b61356230acf 449
suupen 6:b61356230acf 450 void MDX20::offsetYAxisAdjustment(int16_t axisData)
suupen 6:b61356230acf 451 {
suupen 6:b61356230acf 452 XYZMove(0, axisData, 0);
suupen 6:b61356230acf 453 // D_userOriginPosition[Z_y] = 0;
suupen 6:b61356230acf 454
suupen 6:b61356230acf 455 }
suupen 6:b61356230acf 456
suupen 6:b61356230acf 457 void MDX20::offsetZAxisAdjustment(int16_t axisData)
suupen 6:b61356230acf 458 {
suupen 6:b61356230acf 459 XYZMove(0, 0, axisData);
suupen 6:b61356230acf 460 // D_userOriginPosition[Z_z] = 0;
suupen 6:b61356230acf 461
suupen 6:b61356230acf 462 }
suupen 6:b61356230acf 463
suupen 6:b61356230acf 464 void MDX20::userOriginPositionInitial(void)
suupen 6:b61356230acf 465 {
suupen 6:b61356230acf 466 D_userOriginPosition[Z_x] = 0;
suupen 6:b61356230acf 467 D_userOriginPosition[Z_y] = 0;
suupen 6:b61356230acf 468 D_userOriginPosition[Z_z] = 0;
suupen 6:b61356230acf 469 }
suupen 6:b61356230acf 470
suupen 6:b61356230acf 471 int32_t MDX20::motorStateCheck(void)
suupen 6:b61356230acf 472 {
suupen 6:b61356230acf 473 return motorState;
suupen 8:e65e532d1933 474 }
suupen 6:b61356230acf 475
suupen 9:5ee05e9c5aca 476 /** MDX-20の接続確認
suupen 9:5ee05e9c5aca 477 * @return true:接続あり false:なし
suupen 9:5ee05e9c5aca 478 */
suupen 9:5ee05e9c5aca 479 int32_t MDX20::connectCheck(void)
suupen 9:5ee05e9c5aca 480 {
suupen 9:5ee05e9c5aca 481 int32_t ans = false;
suupen 9:5ee05e9c5aca 482
suupen 9:5ee05e9c5aca 483 if(_cts == 0){
suupen 9:5ee05e9c5aca 484 ans = true;
suupen 9:5ee05e9c5aca 485 }else{
suupen 9:5ee05e9c5aca 486 ans = false;
suupen 9:5ee05e9c5aca 487 }
suupen 9:5ee05e9c5aca 488 return (ans);
suupen 9:5ee05e9c5aca 489 }
suupen 9:5ee05e9c5aca 490