suu pen / MDX20

MDX-15,20の制御用library

MDX20.cpp@4:b01a67ab40cf, 2016-11-23 (annotated)

Committer:
suupen
Date:
Wed Nov 23 09:12:01 2016 +0000
Revision:
4:b01a67ab40cf
Parent:
3:3bf2936b8d9b
Child:
5:bd414d8e483f
motorOff???;

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 3:3bf2936b8d9b 50 } else if( 0 == strncmp("^PR", strData, 3)) {
suupen 3:3bf2936b8d9b 51 AorR = 'R';
suupen 3:3bf2936b8d9b 52 }
suupen 3:3bf2936b8d9b 53
suupen 3:3bf2936b8d9b 54
suupen 3:3bf2936b8d9b 55 while ((p = strchr(strData, ','))!=NULL) *p = ' ';
suupen 3:3bf2936b8d9b 56
suupen 3:3bf2936b8d9b 57 double a[3] = {0,0,0};
suupen 3:3bf2936b8d9b 58 if(*str != 'Z') {
suupen 3:3bf2936b8d9b 59 return;
suupen 2:8446eb1774c4 60 }
suupen 2:8446eb1774c4 61
suupen 3:3bf2936b8d9b 62 sscanf((strData + 1), "%lf %lf %lf", &a[0], &a[1], &a[2]);
suupen 3:3bf2936b8d9b 63
suupen 3:3bf2936b8d9b 64 if(AorR == 'A') {
suupen 3:3bf2936b8d9b 65 D_position[Z_x] = a[0];
suupen 3:3bf2936b8d9b 66 D_position[Z_y] = a[1];
suupen 3:3bf2936b8d9b 67 D_position[Z_z] = a[2];
suupen 3:3bf2936b8d9b 68 } else {
suupen 3:3bf2936b8d9b 69 D_position[Z_x] += a[0];
suupen 3:3bf2936b8d9b 70 D_position[Z_y] += a[1];
suupen 3:3bf2936b8d9b 71 D_position[Z_z] += a[2];
suupen 3:3bf2936b8d9b 72 }
suupen 3:3bf2936b8d9b 73
suupen 3:3bf2936b8d9b 74 printf("x=%f y=%f z=%f \n", D_position[Z_x], D_position[Z_y], D_position[Z_z]);
suupen 3:3bf2936b8d9b 75 wait(0.1);
suupen 3:3bf2936b8d9b 76 }
suupen 3:3bf2936b8d9b 77
suupen 3:3bf2936b8d9b 78 uint8_t MDX20::xyOrigin(void)
suupen 2:8446eb1774c4 79 {
suupen 3:3bf2936b8d9b 80 uint8_t ans;
suupen 3:3bf2936b8d9b 81 char buffer[100];
suupen 3:3bf2936b8d9b 82 D_userOriginPosition[Z_x] = D_position[Z_x];
suupen 3:3bf2936b8d9b 83 D_userOriginPosition[Z_y] = D_position[Z_y];
suupen 3:3bf2936b8d9b 84 D_userOriginPosition[Z_z] = 0;
suupen 3:3bf2936b8d9b 85
suupen 3:3bf2936b8d9b 86 sprintf(buffer, "!XO%d;",(int16_t)D_userOriginPosition[Z_x]);
suupen 3:3bf2936b8d9b 87 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 88 sprintf(buffer, "!YO%d;",(int16_t)D_userOriginPosition[Z_y]);
suupen 3:3bf2936b8d9b 89 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 90
suupen 3:3bf2936b8d9b 91 return ans;
suupen 3:3bf2936b8d9b 92 }
suupen 3:3bf2936b8d9b 93
suupen 3:3bf2936b8d9b 94 uint8_t MDX20::zOrigin(void)
suupen 3:3bf2936b8d9b 95 {
suupen 3:3bf2936b8d9b 96 uint8_t ans;
suupen 3:3bf2936b8d9b 97 char buffer[100];
suupen 3:3bf2936b8d9b 98
suupen 3:3bf2936b8d9b 99 sprintf(buffer, "!ZO%d;",0); // 今いる位置をZ原点にするので"0"を設定する
suupen 3:3bf2936b8d9b 100 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 101 ans &= sendData(buffer);
suupen 3:3bf2936b8d9b 102
suupen 3:3bf2936b8d9b 103 return ans;
suupen 3:3bf2936b8d9b 104 }
suupen 2:8446eb1774c4 105
suupen 1:1751fee99a0d 106 /**
suupen 1:1751fee99a0d 107 * MDX-15/20へのデータ送信
suupen 1:1751fee99a0d 108 * @@para *data : データ一行の先頭アドレス
suupen 1:1751fee99a0d 109 * @@para uint8_t : 0:送信キャンセル 1:送信完了
suupen 1:1751fee99a0d 110 */
suupen 1:1751fee99a0d 111 uint8_t MDX20::sendData(char* data)
suupen 1:1751fee99a0d 112 {
suupen 1:1751fee99a0d 113 uint8_t ans = 0; // 0:送信キャンセル 1:送信完了
suupen 1:1751fee99a0d 114
suupen 2:8446eb1774c4 115 while(_cts != 0) {}
suupen 2:8446eb1774c4 116 wait(0.1); // このwait timeがないとMDX-20からのwait指示を読み飛ばす
suupen 2:8446eb1774c4 117 // printf("%s\r\n",data);
suupen 2:8446eb1774c4 118 _serial.printf("%s\r\n",data);
suupen 3:3bf2936b8d9b 119
suupen 3:3bf2936b8d9b 120 integralPosition(data);
suupen 2:8446eb1774c4 121 ans = 1;
suupen 2:8446eb1774c4 122
suupen 1:1751fee99a0d 123 return (ans);
suupen 1:1751fee99a0d 124 }
suupen 1:1751fee99a0d 125
suupen 3:3bf2936b8d9b 126 uint8_t MDX20::reciveData(void)
suupen 3:3bf2936b8d9b 127 {
suupen 3:3bf2936b8d9b 128 char ans = 0;
suupen 3:3bf2936b8d9b 129 while(_serial.readable()) {
suupen 3:3bf2936b8d9b 130 ans = _serial.getc();
suupen 3:3bf2936b8d9b 131 }
suupen 3:3bf2936b8d9b 132 return ans;
suupen 3:3bf2936b8d9b 133 }
suupen 3:3bf2936b8d9b 134
suupen 1:1751fee99a0d 135 int MDX20::putc(int c)
suupen 1:1751fee99a0d 136 {
suupen 1:1751fee99a0d 137 _serial.putc(c);
suupen 1:1751fee99a0d 138 return c;
suupen 1:1751fee99a0d 139 }
suupen 1:1751fee99a0d 140
suupen 2:8446eb1774c4 141 uint8_t MDX20::initial(void)
suupen 2:8446eb1774c4 142 {
suupen 2:8446eb1774c4 143 uint8_t ans;
suupen 3:3bf2936b8d9b 144 ans &= sendData("^IN");
suupen 2:8446eb1774c4 145 ans &= sendData("!MC0");
suupen 2:8446eb1774c4 146 ans &= sendData("^PA");
suupen 2:8446eb1774c4 147 ans &= sendData("Z0,0,0");
suupen 2:8446eb1774c4 148 clearPositon();
suupen 2:8446eb1774c4 149 return (ans);
suupen 2:8446eb1774c4 150 }
suupen 2:8446eb1774c4 151
suupen 4:b01a67ab40cf 152 uint8_t MDX20::motorOff(void)
suupen 4:b01a67ab40cf 153 {
suupen 4:b01a67ab40cf 154 uint8_t ans;
suupen 4:b01a67ab40cf 155 ans &= sendData("!MC0");
suupen 4:b01a67ab40cf 156 return (ans);
suupen 4:b01a67ab40cf 157 }
suupen 4:b01a67ab40cf 158
suupen 3:3bf2936b8d9b 159 uint8_t MDX20::userOriginInitial(void)
suupen 3:3bf2936b8d9b 160 {
suupen 3:3bf2936b8d9b 161 char buffer[100];
suupen 3:3bf2936b8d9b 162 uint8_t ans;
suupen 3:3bf2936b8d9b 163
suupen 3:3bf2936b8d9b 164 ans &= sendData("^PA");
suupen 3:3bf2936b8d9b 165
suupen 3:3bf2936b8d9b 166 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 167 // printf("%s\r\n",buffer);
suupen 3:3bf2936b8d9b 168 ans &= sendData(buffer);
suupen 3:3bf2936b8d9b 169
suupen 3:3bf2936b8d9b 170 return ans;
suupen 3:3bf2936b8d9b 171 }
suupen 3:3bf2936b8d9b 172
suupen 2:8446eb1774c4 173 uint8_t MDX20::final(void)
suupen 2:8446eb1774c4 174 {
suupen 3:3bf2936b8d9b 175 uint8_t ans;
suupen 2:8446eb1774c4 176 ans &= sendData("!MC0");
suupen 2:8446eb1774c4 177 ans &= sendData("^PA");
suupen 2:8446eb1774c4 178 ans &= sendData("Z0,0,0");
suupen 2:8446eb1774c4 179 clearPositon();
suupen 3:3bf2936b8d9b 180 ans &= sendData("^IN");
suupen 2:8446eb1774c4 181 return (ans);
suupen 3:3bf2936b8d9b 182 }
suupen 2:8446eb1774c4 183
suupen 2:8446eb1774c4 184 uint8_t MDX20::zeroSetting(void)
suupen 2:8446eb1774c4 185 {
suupen 2:8446eb1774c4 186 uint8_t ans;
suupen 2:8446eb1774c4 187
suupen 2:8446eb1774c4 188 ans &= sendData("!X00");
suupen 2:8446eb1774c4 189 ans &= sendData("!Y00");
suupen 2:8446eb1774c4 190 ans &= sendData("!Z00");
suupen 2:8446eb1774c4 191 clearPositon();
suupen 2:8446eb1774c4 192 return (ans);
suupen 2:8446eb1774c4 193 }
suupen 2:8446eb1774c4 194 uint8_t MDX20::XYZMove(int16_t x, int16_t y, int16_t z)
suupen 2:8446eb1774c4 195 {
suupen 2:8446eb1774c4 196 uint8_t ans;
suupen 2:8446eb1774c4 197 char buffer[100];
suupen 2:8446eb1774c4 198
suupen 2:8446eb1774c4 199 ans &= sendData("!MC0");
suupen 2:8446eb1774c4 200 ans &= sendData("^PR");
suupen 2:8446eb1774c4 201 sprintf(buffer, "Z%05d,%05d,%05d",x, y, z);
suupen 2:8446eb1774c4 202 ans &= sendData(buffer);
suupen 2:8446eb1774c4 203
suupen 2:8446eb1774c4 204 return (ans);
suupen 2:8446eb1774c4 205 }