suu pen / MDX20

MDX-15,20の制御用library

MDX20.cpp@5:bd414d8e483f, 2016-11-26 (annotated)

Committer:
suupen
Date:
Sat Nov 26 06:41:51 2016 +0000
Revision:
5:bd414d8e483f
Parent:
4:b01a67ab40cf
Child:
6:b61356230acf
?????double??int????????; ;

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 1:1751fee99a0d 4
suupen 1:1751fee99a0d 5 MDX20::MDX20(PinName tx, PinName rx, PinName cts) : _serial(tx, rx), _cts(cts, PullUp)
suupen 1:1751fee99a0d 6 {
suupen 1:1751fee99a0d 7
suupen 1:1751fee99a0d 8 _serial.baud(9600);
suupen 1:1751fee99a0d 9 _serial.format(8,Serial::None,1);
suupen 1:1751fee99a0d 10 }
suupen 1:1751fee99a0d 11
suupen 1:1751fee99a0d 12 MDX20::~MDX20()
suupen 1:1751fee99a0d 13 {
suupen 1:1751fee99a0d 14 }
suupen 1:1751fee99a0d 15
suupen 2:8446eb1774c4 16 void MDX20::clearPositon(void)
suupen 2:8446eb1774c4 17 {
suupen 2:8446eb1774c4 18 D_position[Z_x] = 0;
suupen 2:8446eb1774c4 19 D_position[Z_y] = 0;
suupen 2:8446eb1774c4 20 D_position[Z_z] = 0;
suupen 2:8446eb1774c4 21 }
suupen 3:3bf2936b8d9b 22
suupen 2:8446eb1774c4 23 void MDX20::answerPositon(int16_t *position)
suupen 2:8446eb1774c4 24 {
suupen 2:8446eb1774c4 25 *(position + Z_x) = D_position[Z_x];
suupen 2:8446eb1774c4 26 *(position + Z_y) = D_position[Z_y];
suupen 2:8446eb1774c4 27 *(position + Z_z) = D_position[Z_z];
suupen 2:8446eb1774c4 28
suupen 3:3bf2936b8d9b 29 }
suupen 3:3bf2936b8d9b 30
suupen 3:3bf2936b8d9b 31 void MDX20::answerPositonMillimeter(float *position)
suupen 3:3bf2936b8d9b 32 {
suupen 3:3bf2936b8d9b 33 *(position + Z_x) = (float)D_position[Z_x] * countToMillimeter;
suupen 3:3bf2936b8d9b 34 *(position + Z_y) = (float)D_position[Z_y] * countToMillimeter;
suupen 3:3bf2936b8d9b 35 *(position + Z_z) = (float)D_position[Z_z] * countToMillimeter;
suupen 3:3bf2936b8d9b 36
suupen 3:3bf2936b8d9b 37 }
suupen 3:3bf2936b8d9b 38
suupen 3:3bf2936b8d9b 39
suupen 3:3bf2936b8d9b 40 void MDX20::integralPosition(char *str)
suupen 3:3bf2936b8d9b 41 {
suupen 3:3bf2936b8d9b 42 char strData[100];
suupen 3:3bf2936b8d9b 43 char *p;
suupen 3:3bf2936b8d9b 44 static uint8_t AorR = 'A'; // 'A'=absolute 'R'=relative
suupen 3:3bf2936b8d9b 45
suupen 3:3bf2936b8d9b 46 strcpy(strData, str);
suupen 3:3bf2936b8d9b 47
suupen 3:3bf2936b8d9b 48 if( 0 == strncmp("^PA", strData, 3)) {
suupen 3:3bf2936b8d9b 49 AorR = 'A';
suupen 5:bd414d8e483f 50 return;
suupen 3:3bf2936b8d9b 51 } else if( 0 == strncmp("^PR", strData, 3)) {
suupen 3:3bf2936b8d9b 52 AorR = 'R';
suupen 5:bd414d8e483f 53 return;
suupen 5:bd414d8e483f 54 }
suupen 5:bd414d8e483f 55
suupen 5:bd414d8e483f 56 // コマンド中の","を" "に置き換える
suupen 5:bd414d8e483f 57 while ((p = strchr(strData, ','))!=NULL) *p = ' ';
suupen 5:bd414d8e483f 58
suupen 5:bd414d8e483f 59 double a[3] = {0,0,0};
suupen 5:bd414d8e483f 60
suupen 5:bd414d8e483f 61 if(strncmp(str, "!ZZ",3) == 0) {
suupen 5:bd414d8e483f 62 sscanf((strData + 3), "%lf %lf %lf", &a[0], &a[1], &a[2]);
suupen 5:bd414d8e483f 63 } else if(strncmp(str, "Z",1) == 0) {
suupen 5:bd414d8e483f 64 sscanf((strData + 1), "%lf %lf %lf", &a[0], &a[1], &a[2]);
suupen 5:bd414d8e483f 65 } else if(strncmp(str, "^PU",3) == 0) {
suupen 5:bd414d8e483f 66 sscanf((strData + 3), "%lf %lf", &a[0], &a[1]);
suupen 5:bd414d8e483f 67 a[2] = DBL_MAX;
suupen 5:bd414d8e483f 68 } else if(strncmp(str, "^PD",3) == 0) {
suupen 5:bd414d8e483f 69 sscanf((strData + 3), "%lf %lf", &a[0], &a[1]);
suupen 5:bd414d8e483f 70 a[2] = DBL_MAX;
suupen 3:3bf2936b8d9b 71 }
suupen 3:3bf2936b8d9b 72
suupen 3:3bf2936b8d9b 73
suupen 3:3bf2936b8d9b 74
suupen 3:3bf2936b8d9b 75 if(AorR == 'A') {
suupen 3:3bf2936b8d9b 76 D_position[Z_x] = a[0];
suupen 3:3bf2936b8d9b 77 D_position[Z_y] = a[1];
suupen 5:bd414d8e483f 78 if(a[2] != DBL_MAX){
suupen 3:3bf2936b8d9b 79 D_position[Z_z] = a[2];
suupen 5:bd414d8e483f 80 }
suupen 3:3bf2936b8d9b 81 } else {
suupen 3:3bf2936b8d9b 82 D_position[Z_x] += a[0];
suupen 3:3bf2936b8d9b 83 D_position[Z_y] += a[1];
suupen 5:bd414d8e483f 84 if(a[2] != DBL_MAX){
suupen 3:3bf2936b8d9b 85 D_position[Z_z] += a[2];
suupen 5:bd414d8e483f 86 }
suupen 3:3bf2936b8d9b 87 }
suupen 3:3bf2936b8d9b 88
suupen 3:3bf2936b8d9b 89 printf("x=%f y=%f z=%f \n", D_position[Z_x], D_position[Z_y], D_position[Z_z]);
suupen 3:3bf2936b8d9b 90 wait(0.1);
suupen 3:3bf2936b8d9b 91 }
suupen 3:3bf2936b8d9b 92
suupen 3:3bf2936b8d9b 93 uint8_t MDX20::xyOrigin(void)
suupen 2:8446eb1774c4 94 {
suupen 3:3bf2936b8d9b 95 uint8_t ans;
suupen 3:3bf2936b8d9b 96 char buffer[100];
suupen 3:3bf2936b8d9b 97 D_userOriginPosition[Z_x] = D_position[Z_x];
suupen 3:3bf2936b8d9b 98 D_userOriginPosition[Z_y] = D_position[Z_y];
suupen 5:bd414d8e483f 99 // D_userOriginPosition[Z_z] = 0;
suupen 3:3bf2936b8d9b 100
suupen 3:3bf2936b8d9b 101 sprintf(buffer, "!XO%d;",(int16_t)D_userOriginPosition[Z_x]);
suupen 3:3bf2936b8d9b 102 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 103 sprintf(buffer, "!YO%d;",(int16_t)D_userOriginPosition[Z_y]);
suupen 3:3bf2936b8d9b 104 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 105
suupen 3:3bf2936b8d9b 106 return ans;
suupen 3:3bf2936b8d9b 107 }
suupen 3:3bf2936b8d9b 108
suupen 3:3bf2936b8d9b 109 uint8_t MDX20::zOrigin(void)
suupen 3:3bf2936b8d9b 110 {
suupen 3:3bf2936b8d9b 111 uint8_t ans;
suupen 3:3bf2936b8d9b 112 char buffer[100];
suupen 5:bd414d8e483f 113 D_userOriginPosition[Z_z] = D_position[Z_z];
suupen 5:bd414d8e483f 114 sprintf(buffer, "!ZO%d;",(int16_t)D_userOriginPosition[Z_z]);
suupen 3:3bf2936b8d9b 115
suupen 5:bd414d8e483f 116 /* Z0 command use
suupen 5:bd414d8e483f 117 sprintf(buffer, "!ZO%d;",0); // 今いる位置をZ原点にするので"0"を設定する
suupen 5:bd414d8e483f 118 // printf("%s\r\n",buffer);
suupen 5:bd414d8e483f 119 ans &= sendData(buffer);
suupen 5:bd414d8e483f 120 */
suupen 3:3bf2936b8d9b 121 return ans;
suupen 3:3bf2936b8d9b 122 }
suupen 2:8446eb1774c4 123
suupen 1:1751fee99a0d 124 /**
suupen 1:1751fee99a0d 125 * MDX-15/20へのデータ送信
suupen 1:1751fee99a0d 126 * @@para *data : データ一行の先頭アドレス
suupen 1:1751fee99a0d 127 * @@para uint8_t : 0:送信キャンセル 1:送信完了
suupen 1:1751fee99a0d 128 */
suupen 1:1751fee99a0d 129 uint8_t MDX20::sendData(char* data)
suupen 1:1751fee99a0d 130 {
suupen 1:1751fee99a0d 131 uint8_t ans = 0; // 0:送信キャンセル 1:送信完了
suupen 1:1751fee99a0d 132
suupen 2:8446eb1774c4 133 while(_cts != 0) {}
suupen 2:8446eb1774c4 134 wait(0.1); // このwait timeがないとMDX-20からのwait指示を読み飛ばす
suupen 2:8446eb1774c4 135 // printf("%s\r\n",data);
suupen 2:8446eb1774c4 136 _serial.printf("%s\r\n",data);
suupen 3:3bf2936b8d9b 137
suupen 3:3bf2936b8d9b 138 integralPosition(data);
suupen 2:8446eb1774c4 139 ans = 1;
suupen 2:8446eb1774c4 140
suupen 1:1751fee99a0d 141 return (ans);
suupen 1:1751fee99a0d 142 }
suupen 1:1751fee99a0d 143
suupen 3:3bf2936b8d9b 144 uint8_t MDX20::reciveData(void)
suupen 3:3bf2936b8d9b 145 {
suupen 3:3bf2936b8d9b 146 char ans = 0;
suupen 3:3bf2936b8d9b 147 while(_serial.readable()) {
suupen 3:3bf2936b8d9b 148 ans = _serial.getc();
suupen 3:3bf2936b8d9b 149 }
suupen 3:3bf2936b8d9b 150 return ans;
suupen 3:3bf2936b8d9b 151 }
suupen 3:3bf2936b8d9b 152
suupen 1:1751fee99a0d 153 int MDX20::putc(int c)
suupen 1:1751fee99a0d 154 {
suupen 1:1751fee99a0d 155 _serial.putc(c);
suupen 1:1751fee99a0d 156 return c;
suupen 1:1751fee99a0d 157 }
suupen 1:1751fee99a0d 158
suupen 2:8446eb1774c4 159 uint8_t MDX20::initial(void)
suupen 2:8446eb1774c4 160 {
suupen 2:8446eb1774c4 161 uint8_t ans;
suupen 3:3bf2936b8d9b 162 ans &= sendData("^IN");
suupen 2:8446eb1774c4 163 ans &= sendData("!MC0");
suupen 2:8446eb1774c4 164 ans &= sendData("^PA");
suupen 2:8446eb1774c4 165 ans &= sendData("Z0,0,0");
suupen 2:8446eb1774c4 166 clearPositon();
suupen 2:8446eb1774c4 167 return (ans);
suupen 2:8446eb1774c4 168 }
suupen 2:8446eb1774c4 169
suupen 4:b01a67ab40cf 170 uint8_t MDX20::motorOff(void)
suupen 4:b01a67ab40cf 171 {
suupen 4:b01a67ab40cf 172 uint8_t ans;
suupen 4:b01a67ab40cf 173 ans &= sendData("!MC0");
suupen 4:b01a67ab40cf 174 return (ans);
suupen 4:b01a67ab40cf 175 }
suupen 4:b01a67ab40cf 176
suupen 3:3bf2936b8d9b 177 uint8_t MDX20::userOriginInitial(void)
suupen 3:3bf2936b8d9b 178 {
suupen 3:3bf2936b8d9b 179 char buffer[100];
suupen 3:3bf2936b8d9b 180 uint8_t ans;
suupen 3:3bf2936b8d9b 181
suupen 3:3bf2936b8d9b 182 ans &= sendData("^PA");
suupen 3:3bf2936b8d9b 183
suupen 3:3bf2936b8d9b 184 sprintf(buffer, "Z%d,%d,%d",(int16_t)D_userOriginPosition[Z_x], (int16_t)D_userOriginPosition[Z_y], (int16_t)D_userOriginPosition[Z_z]);
suupen 3:3bf2936b8d9b 185 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 186 ans &= sendData(buffer);
suupen 3:3bf2936b8d9b 187
suupen 3:3bf2936b8d9b 188 return ans;
suupen 3:3bf2936b8d9b 189 }
suupen 3:3bf2936b8d9b 190
suupen 2:8446eb1774c4 191 uint8_t MDX20::final(void)
suupen 2:8446eb1774c4 192 {
suupen 3:3bf2936b8d9b 193 uint8_t ans;
suupen 2:8446eb1774c4 194 ans &= sendData("!MC0");
suupen 2:8446eb1774c4 195 ans &= sendData("^PA");
suupen 2:8446eb1774c4 196 ans &= sendData("Z0,0,0");
suupen 2:8446eb1774c4 197 clearPositon();
suupen 3:3bf2936b8d9b 198 ans &= sendData("^IN");
suupen 2:8446eb1774c4 199 return (ans);
suupen 3:3bf2936b8d9b 200 }
suupen 5:bd414d8e483f 201 /*
suupen 2:8446eb1774c4 202 uint8_t MDX20::zeroSetting(void)
suupen 2:8446eb1774c4 203 {
suupen 2:8446eb1774c4 204 uint8_t ans;
suupen 2:8446eb1774c4 205
suupen 2:8446eb1774c4 206 ans &= sendData("!X00");
suupen 2:8446eb1774c4 207 ans &= sendData("!Y00");
suupen 2:8446eb1774c4 208 ans &= sendData("!Z00");
suupen 2:8446eb1774c4 209 clearPositon();
suupen 2:8446eb1774c4 210 return (ans);
suupen 2:8446eb1774c4 211 }
suupen 5:bd414d8e483f 212 */
suupen 5:bd414d8e483f 213
suupen 2:8446eb1774c4 214 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
suupen 2:8446eb1774c4 215 {
suupen 2:8446eb1774c4 216 uint8_t ans;
suupen 2:8446eb1774c4 217 char buffer[100];
suupen 2:8446eb1774c4 218
suupen 2:8446eb1774c4 219 ans &= sendData("!MC0");
suupen 2:8446eb1774c4 220 ans &= sendData("^PR");
suupen 2:8446eb1774c4 221 sprintf(buffer, "Z%05d,%05d,%05d",x, y, z);
suupen 2:8446eb1774c4 222 ans &= sendData(buffer);
suupen 2:8446eb1774c4 223
suupen 2:8446eb1774c4 224 return (ans);
suupen 2:8446eb1774c4 225 }