MDX-15,20の制御用library
MDX20.cpp@8:e65e532d1933, 2016-12-03 (annotated)
- Committer:
- suupen
- Date:
- Sat Dec 03 13:21:20 2016 +0000
- Revision:
- 8:e65e532d1933
- Parent:
- 7:54af1616e69e
- Child:
- 9:5ee05e9c5aca
?????????
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:b61356230acf | 257 | |
suupen | 3:3bf2936b8d9b | 258 | uint8_t MDX20::reciveData(void) |
suupen | 3:3bf2936b8d9b | 259 | { |
suupen | 3:3bf2936b8d9b | 260 | char ans = 0; |
suupen | 3:3bf2936b8d9b | 261 | while(_serial.readable()) { |
suupen | 3:3bf2936b8d9b | 262 | ans = _serial.getc(); |
suupen | 3:3bf2936b8d9b | 263 | } |
suupen | 3:3bf2936b8d9b | 264 | return ans; |
suupen | 3:3bf2936b8d9b | 265 | } |
suupen | 3:3bf2936b8d9b | 266 | |
suupen | 1:1751fee99a0d | 267 | int MDX20::putc(int c) |
suupen | 1:1751fee99a0d | 268 | { |
suupen | 1:1751fee99a0d | 269 | _serial.putc(c); |
suupen | 1:1751fee99a0d | 270 | return c; |
suupen | 1:1751fee99a0d | 271 | } |
suupen | 1:1751fee99a0d | 272 | |
suupen | 2:8446eb1774c4 | 273 | uint8_t MDX20::initial(void) |
suupen | 2:8446eb1774c4 | 274 | { |
suupen | 2:8446eb1774c4 | 275 | uint8_t ans; |
suupen | 6:b61356230acf | 276 | char buffer[50]; |
suupen | 2:8446eb1774c4 | 277 | clearPositon(); |
suupen | 6:b61356230acf | 278 | FILE *fp; |
suupen | 6:b61356230acf | 279 | |
suupen | 6:b61356230acf | 280 | fp = fopen("/local/ORIGINX.INI", "r"); |
suupen | 6:b61356230acf | 281 | if ( fp != NULL ) { |
suupen | 6:b61356230acf | 282 | if(NULL != fgets(buffer, sizeof(buffer), fp)) { |
suupen | 6:b61356230acf | 283 | D_userOriginPosition[Z_x] = atoi(buffer); |
suupen | 6:b61356230acf | 284 | } else { |
suupen | 6:b61356230acf | 285 | // nothing |
suupen | 6:b61356230acf | 286 | } |
suupen | 6:b61356230acf | 287 | fclose(fp); |
suupen | 6:b61356230acf | 288 | |
suupen | 6:b61356230acf | 289 | } else { |
suupen | 6:b61356230acf | 290 | // nothing |
suupen | 6:b61356230acf | 291 | } |
suupen | 6:b61356230acf | 292 | |
suupen | 6:b61356230acf | 293 | fp = fopen("/local/ORIGINY.INI", "r"); |
suupen | 6:b61356230acf | 294 | if ( fp != NULL ) { |
suupen | 6:b61356230acf | 295 | if(NULL != fgets(buffer, sizeof(buffer), fp)) { |
suupen | 6:b61356230acf | 296 | D_userOriginPosition[Z_y] = atoi(buffer); |
suupen | 6:b61356230acf | 297 | } else { |
suupen | 6:b61356230acf | 298 | // nothing |
suupen | 6:b61356230acf | 299 | } |
suupen | 6:b61356230acf | 300 | fclose(fp); |
suupen | 6:b61356230acf | 301 | |
suupen | 6:b61356230acf | 302 | |
suupen | 6:b61356230acf | 303 | } else { |
suupen | 6:b61356230acf | 304 | // nothing |
suupen | 6:b61356230acf | 305 | } |
suupen | 6:b61356230acf | 306 | |
suupen | 6:b61356230acf | 307 | fp = fopen("/local/ORIGINZ.INI", "r"); |
suupen | 6:b61356230acf | 308 | if ( fp != NULL ) { |
suupen | 6:b61356230acf | 309 | if(NULL != fgets(buffer, sizeof(buffer), fp)) { |
suupen | 6:b61356230acf | 310 | D_userOriginPosition[Z_z] = atoi(buffer); |
suupen | 6:b61356230acf | 311 | } else { |
suupen | 6:b61356230acf | 312 | // nothing |
suupen | 6:b61356230acf | 313 | } |
suupen | 6:b61356230acf | 314 | fclose(fp); |
suupen | 6:b61356230acf | 315 | |
suupen | 6:b61356230acf | 316 | } else { |
suupen | 6:b61356230acf | 317 | // nothing |
suupen | 6:b61356230acf | 318 | } |
suupen | 6:b61356230acf | 319 | |
suupen | 6:b61356230acf | 320 | ans &= sendData("^IN;"); |
suupen | 6:b61356230acf | 321 | ans &= sendData("!MC0;"); |
suupen | 6:b61356230acf | 322 | ans &= sendData("!ZO0;"); |
suupen | 6:b61356230acf | 323 | ans &= sendData("!ZO0;"); // Z axis origin initialaize |
suupen | 6:b61356230acf | 324 | ans &= sendData("^PA;"); |
suupen | 6:b61356230acf | 325 | |
suupen | 6:b61356230acf | 326 | sprintf(buffer,"Z%d,%d,%d;",-D_userOriginPosition[Z_x], -D_userOriginPosition[Z_y], -D_userOriginPosition[Z_z]); |
suupen | 6:b61356230acf | 327 | |
suupen | 6:b61356230acf | 328 | ans &= sendData(buffer); |
suupen | 6:b61356230acf | 329 | |
suupen | 2:8446eb1774c4 | 330 | return (ans); |
suupen | 2:8446eb1774c4 | 331 | } |
suupen | 2:8446eb1774c4 | 332 | |
suupen | 4:b01a67ab40cf | 333 | uint8_t MDX20::motorOff(void) |
suupen | 4:b01a67ab40cf | 334 | { |
suupen | 4:b01a67ab40cf | 335 | uint8_t ans; |
suupen | 6:b61356230acf | 336 | ans &= sendData("!MC0;"); |
suupen | 4:b01a67ab40cf | 337 | return (ans); |
suupen | 4:b01a67ab40cf | 338 | } |
suupen | 4:b01a67ab40cf | 339 | |
suupen | 6:b61356230acf | 340 | uint8_t MDX20::motorOn(void) |
suupen | 6:b61356230acf | 341 | { |
suupen | 6:b61356230acf | 342 | uint8_t ans; |
suupen | 8:e65e532d1933 | 343 | ans &= sendData("!MC1;"); |
suupen | 8:e65e532d1933 | 344 | ans &= sendData("^PR;"); |
suupen | 8:e65e532d1933 | 345 | ans &= sendData("Z0,0,0"); |
suupen | 6:b61356230acf | 346 | return (ans); |
suupen | 6:b61356230acf | 347 | } |
suupen | 6:b61356230acf | 348 | |
suupen | 6:b61356230acf | 349 | |
suupen | 3:3bf2936b8d9b | 350 | uint8_t MDX20::userOriginInitial(void) |
suupen | 3:3bf2936b8d9b | 351 | { |
suupen | 6:b61356230acf | 352 | char buffer[50]; |
suupen | 3:3bf2936b8d9b | 353 | uint8_t ans; |
suupen | 3:3bf2936b8d9b | 354 | |
suupen | 6:b61356230acf | 355 | ans &= sendData("^PA;"); |
suupen | 6:b61356230acf | 356 | ans &= sendData("!MC0;"); |
suupen | 3:3bf2936b8d9b | 357 | |
suupen | 6:b61356230acf | 358 | sprintf(buffer, "Z%d,%d,%d;",0, 0, 0); |
suupen | 3:3bf2936b8d9b | 359 | ans &= sendData(buffer); |
suupen | 3:3bf2936b8d9b | 360 | |
suupen | 3:3bf2936b8d9b | 361 | return ans; |
suupen | 3:3bf2936b8d9b | 362 | } |
suupen | 3:3bf2936b8d9b | 363 | |
suupen | 2:8446eb1774c4 | 364 | uint8_t MDX20::final(void) |
suupen | 2:8446eb1774c4 | 365 | { |
suupen | 3:3bf2936b8d9b | 366 | uint8_t ans; |
suupen | 6:b61356230acf | 367 | ans &= sendData("!MC0;"); |
suupen | 6:b61356230acf | 368 | ans &= sendData("^PA;"); |
suupen | 6:b61356230acf | 369 | ans &= sendData("Z0,0,0;"); |
suupen | 2:8446eb1774c4 | 370 | clearPositon(); |
suupen | 6:b61356230acf | 371 | ans &= sendData("^IN;"); |
suupen | 2:8446eb1774c4 | 372 | return (ans); |
suupen | 3:3bf2936b8d9b | 373 | } |
suupen | 5:bd414d8e483f | 374 | /* |
suupen | 2:8446eb1774c4 | 375 | uint8_t MDX20::zeroSetting(void) |
suupen | 2:8446eb1774c4 | 376 | { |
suupen | 2:8446eb1774c4 | 377 | uint8_t ans; |
suupen | 2:8446eb1774c4 | 378 | |
suupen | 2:8446eb1774c4 | 379 | ans &= sendData("!X00"); |
suupen | 2:8446eb1774c4 | 380 | ans &= sendData("!Y00"); |
suupen | 2:8446eb1774c4 | 381 | ans &= sendData("!Z00"); |
suupen | 2:8446eb1774c4 | 382 | clearPositon(); |
suupen | 2:8446eb1774c4 | 383 | return (ans); |
suupen | 2:8446eb1774c4 | 384 | } |
suupen | 5:bd414d8e483f | 385 | */ |
suupen | 5:bd414d8e483f | 386 | |
suupen | 2:8446eb1774c4 | 387 | uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z) |
suupen | 2:8446eb1774c4 | 388 | { |
suupen | 2:8446eb1774c4 | 389 | uint8_t ans; |
suupen | 6:b61356230acf | 390 | char buffer[50]; |
suupen | 2:8446eb1774c4 | 391 | |
suupen | 6:b61356230acf | 392 | ans &= sendData("!MC0;"); |
suupen | 6:b61356230acf | 393 | strcpy(buffer, "^PR;"); |
suupen | 6:b61356230acf | 394 | ans &= sendData(buffer); |
suupen | 6:b61356230acf | 395 | sprintf(buffer, "Z%05d,%05d,%05d;",x, y, z); |
suupen | 2:8446eb1774c4 | 396 | ans &= sendData(buffer); |
suupen | 2:8446eb1774c4 | 397 | |
suupen | 2:8446eb1774c4 | 398 | return (ans); |
suupen | 2:8446eb1774c4 | 399 | } |
suupen | 6:b61356230acf | 400 | |
suupen | 6:b61356230acf | 401 | void MDX20::offsetXAxisAdjustment(int16_t axisData) |
suupen | 6:b61356230acf | 402 | { |
suupen | 6:b61356230acf | 403 | XYZMove(axisData, 0, 0); |
suupen | 6:b61356230acf | 404 | // D_userOriginPosition[Z_x] = 0; |
suupen | 6:b61356230acf | 405 | |
suupen | 6:b61356230acf | 406 | } |
suupen | 6:b61356230acf | 407 | |
suupen | 6:b61356230acf | 408 | void MDX20::offsetYAxisAdjustment(int16_t axisData) |
suupen | 6:b61356230acf | 409 | { |
suupen | 6:b61356230acf | 410 | XYZMove(0, axisData, 0); |
suupen | 6:b61356230acf | 411 | // D_userOriginPosition[Z_y] = 0; |
suupen | 6:b61356230acf | 412 | |
suupen | 6:b61356230acf | 413 | } |
suupen | 6:b61356230acf | 414 | |
suupen | 6:b61356230acf | 415 | void MDX20::offsetZAxisAdjustment(int16_t axisData) |
suupen | 6:b61356230acf | 416 | { |
suupen | 6:b61356230acf | 417 | XYZMove(0, 0, axisData); |
suupen | 6:b61356230acf | 418 | // D_userOriginPosition[Z_z] = 0; |
suupen | 6:b61356230acf | 419 | |
suupen | 6:b61356230acf | 420 | } |
suupen | 6:b61356230acf | 421 | |
suupen | 6:b61356230acf | 422 | void MDX20::userOriginPositionInitial(void) |
suupen | 6:b61356230acf | 423 | { |
suupen | 6:b61356230acf | 424 | D_userOriginPosition[Z_x] = 0; |
suupen | 6:b61356230acf | 425 | D_userOriginPosition[Z_y] = 0; |
suupen | 6:b61356230acf | 426 | D_userOriginPosition[Z_z] = 0; |
suupen | 6:b61356230acf | 427 | } |
suupen | 6:b61356230acf | 428 | |
suupen | 6:b61356230acf | 429 | int32_t MDX20::motorStateCheck(void) |
suupen | 6:b61356230acf | 430 | { |
suupen | 6:b61356230acf | 431 | return motorState; |
suupen | 8:e65e532d1933 | 432 | } |
suupen | 6:b61356230acf | 433 |