suu pen / MDX20

MDX-15,20の制御用library

MDX20.cpp@6:b61356230acf, 2016-11-27 (annotated)

Committer:
suupen
Date:
Sun Nov 27 12:14:47 2016 +0000
Revision:
6:b61356230acf
Parent:
5:bd414d8e483f
Child:
7:54af1616e69e
motor control??????????;

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 6:b61356230acf 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 3:3bf2936b8d9b 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 1:1751fee99a0d 186 /**
suupen 1:1751fee99a0d 187 * MDX-15/20へのデータ送信
suupen 1:1751fee99a0d 188 * @@para *data : データ一行の先頭アドレス
suupen 1:1751fee99a0d 189 * @@para uint8_t : 0:送信キャンセル 1:送信完了
suupen 1:1751fee99a0d 190 */
suupen 1:1751fee99a0d 191 uint8_t MDX20::sendData(char* data)
suupen 1:1751fee99a0d 192 {
suupen 1:1751fee99a0d 193 uint8_t ans = 0; // 0:送信キャンセル 1:送信完了
suupen 6:b61356230acf 194 char buffer[50];
suupen 6:b61356230acf 195 strcpy(buffer, data);
suupen 1:1751fee99a0d 196
suupen 2:8446eb1774c4 197 while(_cts != 0) {}
suupen 2:8446eb1774c4 198 wait(0.1); // このwait timeがないとMDX-20からのwait指示を読み飛ばす
suupen 3:3bf2936b8d9b 199
suupen 6:b61356230acf 200 if(strncmp(buffer, "!MC0", 4) == 0) {
suupen 6:b61356230acf 201 motorState = false;
suupen 6:b61356230acf 202 } else if(strncmp(buffer, "!MC1", 4) == 0) {
suupen 6:b61356230acf 203 motorState = true;
suupen 6:b61356230acf 204 } else {
suupen 6:b61356230acf 205 // nothing
suupen 6:b61356230acf 206 }
suupen 6:b61356230acf 207
suupen 6:b61356230acf 208 DEBUG_PRINT("send = %s\r\n",buffer);
suupen 6:b61356230acf 209 integralPosition(buffer);
suupen 6:b61356230acf 210 _serial.printf("%s\r\n",buffer);
suupen 6:b61356230acf 211
suupen 2:8446eb1774c4 212 ans = 1;
suupen 2:8446eb1774c4 213
suupen 1:1751fee99a0d 214 return (ans);
suupen 1:1751fee99a0d 215 }
suupen 1:1751fee99a0d 216
suupen 6:b61356230acf 217
suupen 3:3bf2936b8d9b 218 uint8_t MDX20::reciveData(void)
suupen 3:3bf2936b8d9b 219 {
suupen 3:3bf2936b8d9b 220 char ans = 0;
suupen 3:3bf2936b8d9b 221 while(_serial.readable()) {
suupen 3:3bf2936b8d9b 222 ans = _serial.getc();
suupen 3:3bf2936b8d9b 223 }
suupen 3:3bf2936b8d9b 224 return ans;
suupen 3:3bf2936b8d9b 225 }
suupen 3:3bf2936b8d9b 226
suupen 1:1751fee99a0d 227 int MDX20::putc(int c)
suupen 1:1751fee99a0d 228 {
suupen 1:1751fee99a0d 229 _serial.putc(c);
suupen 1:1751fee99a0d 230 return c;
suupen 1:1751fee99a0d 231 }
suupen 1:1751fee99a0d 232
suupen 2:8446eb1774c4 233 uint8_t MDX20::initial(void)
suupen 2:8446eb1774c4 234 {
suupen 2:8446eb1774c4 235 uint8_t ans;
suupen 6:b61356230acf 236 char buffer[50];
suupen 2:8446eb1774c4 237 clearPositon();
suupen 6:b61356230acf 238 FILE *fp;
suupen 6:b61356230acf 239
suupen 6:b61356230acf 240 fp = fopen("/local/ORIGINX.INI", "r");
suupen 6:b61356230acf 241 if ( fp != NULL ) {
suupen 6:b61356230acf 242 if(NULL != fgets(buffer, sizeof(buffer), fp)) {
suupen 6:b61356230acf 243 D_userOriginPosition[Z_x] = atoi(buffer);
suupen 6:b61356230acf 244 } else {
suupen 6:b61356230acf 245 // nothing
suupen 6:b61356230acf 246 }
suupen 6:b61356230acf 247 fclose(fp);
suupen 6:b61356230acf 248
suupen 6:b61356230acf 249 } else {
suupen 6:b61356230acf 250 // nothing
suupen 6:b61356230acf 251 }
suupen 6:b61356230acf 252
suupen 6:b61356230acf 253 fp = fopen("/local/ORIGINY.INI", "r");
suupen 6:b61356230acf 254 if ( fp != NULL ) {
suupen 6:b61356230acf 255 if(NULL != fgets(buffer, sizeof(buffer), fp)) {
suupen 6:b61356230acf 256 D_userOriginPosition[Z_y] = atoi(buffer);
suupen 6:b61356230acf 257 } else {
suupen 6:b61356230acf 258 // nothing
suupen 6:b61356230acf 259 }
suupen 6:b61356230acf 260 fclose(fp);
suupen 6:b61356230acf 261
suupen 6:b61356230acf 262
suupen 6:b61356230acf 263 } else {
suupen 6:b61356230acf 264 // nothing
suupen 6:b61356230acf 265 }
suupen 6:b61356230acf 266
suupen 6:b61356230acf 267 fp = fopen("/local/ORIGINZ.INI", "r");
suupen 6:b61356230acf 268 if ( fp != NULL ) {
suupen 6:b61356230acf 269 if(NULL != fgets(buffer, sizeof(buffer), fp)) {
suupen 6:b61356230acf 270 D_userOriginPosition[Z_z] = atoi(buffer);
suupen 6:b61356230acf 271 } else {
suupen 6:b61356230acf 272 // nothing
suupen 6:b61356230acf 273 }
suupen 6:b61356230acf 274 fclose(fp);
suupen 6:b61356230acf 275
suupen 6:b61356230acf 276 } else {
suupen 6:b61356230acf 277 // nothing
suupen 6:b61356230acf 278 }
suupen 6:b61356230acf 279
suupen 6:b61356230acf 280 ans &= sendData("^IN;");
suupen 6:b61356230acf 281 ans &= sendData("!MC0;");
suupen 6:b61356230acf 282 ans &= sendData("!ZO0;");
suupen 6:b61356230acf 283 ans &= sendData("!ZO0;"); // Z axis origin initialaize
suupen 6:b61356230acf 284 ans &= sendData("^PA;");
suupen 6:b61356230acf 285
suupen 6:b61356230acf 286 sprintf(buffer,"Z%d,%d,%d;",-D_userOriginPosition[Z_x], -D_userOriginPosition[Z_y], -D_userOriginPosition[Z_z]);
suupen 6:b61356230acf 287
suupen 6:b61356230acf 288 ans &= sendData(buffer);
suupen 6:b61356230acf 289
suupen 2:8446eb1774c4 290 return (ans);
suupen 2:8446eb1774c4 291 }
suupen 2:8446eb1774c4 292
suupen 4:b01a67ab40cf 293 uint8_t MDX20::motorOff(void)
suupen 4:b01a67ab40cf 294 {
suupen 4:b01a67ab40cf 295 uint8_t ans;
suupen 6:b61356230acf 296 ans &= sendData("!MC0;");
suupen 4:b01a67ab40cf 297 return (ans);
suupen 4:b01a67ab40cf 298 }
suupen 4:b01a67ab40cf 299
suupen 6:b61356230acf 300 uint8_t MDX20::motorOn(void)
suupen 6:b61356230acf 301 {
suupen 6:b61356230acf 302 uint8_t ans;
suupen 6:b61356230acf 303 ans &= sendData("!MC1;");
suupen 6:b61356230acf 304 return (ans);
suupen 6:b61356230acf 305 }
suupen 6:b61356230acf 306
suupen 6:b61356230acf 307
suupen 3:3bf2936b8d9b 308 uint8_t MDX20::userOriginInitial(void)
suupen 3:3bf2936b8d9b 309 {
suupen 6:b61356230acf 310 char buffer[50];
suupen 3:3bf2936b8d9b 311 uint8_t ans;
suupen 3:3bf2936b8d9b 312
suupen 6:b61356230acf 313 ans &= sendData("^PA;");
suupen 6:b61356230acf 314 ans &= sendData("!MC0;");
suupen 3:3bf2936b8d9b 315
suupen 6:b61356230acf 316 sprintf(buffer, "Z%d,%d,%d;",0, 0, 0);
suupen 3:3bf2936b8d9b 317 ans &= sendData(buffer);
suupen 3:3bf2936b8d9b 318
suupen 3:3bf2936b8d9b 319 return ans;
suupen 3:3bf2936b8d9b 320 }
suupen 3:3bf2936b8d9b 321
suupen 2:8446eb1774c4 322 uint8_t MDX20::final(void)
suupen 2:8446eb1774c4 323 {
suupen 3:3bf2936b8d9b 324 uint8_t ans;
suupen 6:b61356230acf 325 ans &= sendData("!MC0;");
suupen 6:b61356230acf 326 ans &= sendData("^PA;");
suupen 6:b61356230acf 327 ans &= sendData("Z0,0,0;");
suupen 2:8446eb1774c4 328 clearPositon();
suupen 6:b61356230acf 329 ans &= sendData("^IN;");
suupen 2:8446eb1774c4 330 return (ans);
suupen 3:3bf2936b8d9b 331 }
suupen 5:bd414d8e483f 332 /*
suupen 2:8446eb1774c4 333 uint8_t MDX20::zeroSetting(void)
suupen 2:8446eb1774c4 334 {
suupen 2:8446eb1774c4 335 uint8_t ans;
suupen 2:8446eb1774c4 336
suupen 2:8446eb1774c4 337 ans &= sendData("!X00");
suupen 2:8446eb1774c4 338 ans &= sendData("!Y00");
suupen 2:8446eb1774c4 339 ans &= sendData("!Z00");
suupen 2:8446eb1774c4 340 clearPositon();
suupen 2:8446eb1774c4 341 return (ans);
suupen 2:8446eb1774c4 342 }
suupen 5:bd414d8e483f 343 */
suupen 5:bd414d8e483f 344
suupen 2:8446eb1774c4 345 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
suupen 2:8446eb1774c4 346 {
suupen 2:8446eb1774c4 347 uint8_t ans;
suupen 6:b61356230acf 348 char buffer[50];
suupen 2:8446eb1774c4 349
suupen 6:b61356230acf 350 ans &= sendData("!MC0;");
suupen 6:b61356230acf 351 strcpy(buffer, "^PR;");
suupen 6:b61356230acf 352 ans &= sendData(buffer);
suupen 6:b61356230acf 353 sprintf(buffer, "Z%05d,%05d,%05d;",x, y, z);
suupen 2:8446eb1774c4 354 ans &= sendData(buffer);
suupen 2:8446eb1774c4 355
suupen 2:8446eb1774c4 356 return (ans);
suupen 2:8446eb1774c4 357 }
suupen 6:b61356230acf 358
suupen 6:b61356230acf 359 void MDX20::offsetXAxisAdjustment(int16_t axisData)
suupen 6:b61356230acf 360 {
suupen 6:b61356230acf 361 XYZMove(axisData, 0, 0);
suupen 6:b61356230acf 362 // D_userOriginPosition[Z_x] = 0;
suupen 6:b61356230acf 363
suupen 6:b61356230acf 364 }
suupen 6:b61356230acf 365
suupen 6:b61356230acf 366 void MDX20::offsetYAxisAdjustment(int16_t axisData)
suupen 6:b61356230acf 367 {
suupen 6:b61356230acf 368 XYZMove(0, axisData, 0);
suupen 6:b61356230acf 369 // D_userOriginPosition[Z_y] = 0;
suupen 6:b61356230acf 370
suupen 6:b61356230acf 371 }
suupen 6:b61356230acf 372
suupen 6:b61356230acf 373 void MDX20::offsetZAxisAdjustment(int16_t axisData)
suupen 6:b61356230acf 374 {
suupen 6:b61356230acf 375 XYZMove(0, 0, axisData);
suupen 6:b61356230acf 376 // D_userOriginPosition[Z_z] = 0;
suupen 6:b61356230acf 377
suupen 6:b61356230acf 378 }
suupen 6:b61356230acf 379
suupen 6:b61356230acf 380 void MDX20::userOriginPositionInitial(void)
suupen 6:b61356230acf 381 {
suupen 6:b61356230acf 382 D_userOriginPosition[Z_x] = 0;
suupen 6:b61356230acf 383 D_userOriginPosition[Z_y] = 0;
suupen 6:b61356230acf 384 D_userOriginPosition[Z_z] = 0;
suupen 6:b61356230acf 385 }
suupen 6:b61356230acf 386
suupen 6:b61356230acf 387 int32_t MDX20::motorStateCheck(void)
suupen 6:b61356230acf 388 {
suupen 6:b61356230acf 389 return motorState;
suupen 6:b61356230acf 390 }
suupen 6:b61356230acf 391