22 #include "../../include/KNI/kmlMotBase.h" 30 # include "keyboard.h" 37 std::cout << std::endl;
38 std::cout <<
"-------------------------------------------" << std::endl;
39 std::cout <<
"Esc: quit program" << std::endl;
40 std::cout <<
"?: Display this help" << std::endl;
41 std::cout <<
"j: Calibrate the Katana" << std::endl;
42 std::cout <<
"c: Set target position, freeze or switch motor off (C Command)" << std::endl;
43 std::cout <<
"C: Set encoder offset (C Command (motNr. + 128))" << std::endl;
44 std::cout <<
"d: Read the position, velocity and pwm duty cycle of a motor (D Command)" << std::endl;
45 std::cout <<
"s: Set and read parameters and limits (S Command)" << std::endl;
46 std::cout <<
"n: Get all motors command flags or positions at once (N Command)" << std::endl;
47 std::cout <<
"g: Set the single polynomial movement parameters (G Command)" << std::endl;
48 std::cout <<
"G: Start the single polynomial movement (G+128 Command)" << std::endl;
49 std::cout <<
"a: Switch collision detection On and Off (A Command)" << std::endl;
50 std::cout <<
"z: Read Echo (Z Command)" << std::endl;
51 std::cout <<
"y: Read the Identification String (Y Command)" << std::endl;
52 std::cout <<
"x: Read the Katana Command Table (X Command)" << std::endl;
53 std::cout <<
"e: Read Gripper Sensor Data (E Command)" << std::endl;
54 std::cout <<
"b: Read the Master firmware-version (B Command)" << std::endl;
55 std::cout <<
"v: Read the Slave firmware-version (V Command)" << std::endl;
56 std::cout <<
"i: Get motCommand from slave (I Command)" << std::endl;
57 std::cout <<
"m: Modbus read and write (M Command)" << std::endl;
58 std::cout <<
"t: Katana 1.2 I/O (T Command)" << std::endl;
59 std::cout <<
"o: Navigation Control (O Command)" << std::endl;
60 std::cout <<
"-------------------------------------------" << std::endl;
65 return (((
short) hi) << 8) + ((short) lo);
68 int main(
int argc,
char *argv[]) {
71 std::cout <<
"usage for socketcommands: socketcommands CONFIGFILE" 72 <<
" IP_ADDR" << std::endl;
76 std::cout <<
"---------------------------" << std::endl;
77 std::cout <<
"SOCKETCOMMANDS DEMO STARTED" << std::endl;
78 std::cout <<
"---------------------------" << std::endl;
85 std::unique_ptr<CCdlSocket>
device;
86 std::unique_ptr<CCplSerialCRC>
protocol;
93 std::cout <<
"-------------------------------------------" << std::endl;
94 std::cout <<
"success: port " << port <<
" open" << std::endl;
95 std::cout <<
"-------------------------------------------" << std::endl;
102 protocol->init(device.get());
103 std::cout <<
"-------------------------------------------" << std::endl;
104 std::cout <<
"success: protocol initiated" << std::endl;
105 std::cout <<
"-------------------------------------------" << std::endl;
113 katana->create(argv[1], protocol.get());
117 std::cout <<
"ERROR: " << e.
message() << std::endl;
120 std::cout <<
"-------------------------------------------" << std::endl;
121 std::cout <<
"success: katana initiated" << std::endl;
122 std::cout <<
"-------------------------------------------" << std::endl;
130 short param1, param2, param3, param4, param5, param6, param7;
151 std::cout << std::endl;
152 std::cout <<
"Calibrating Katana... ";
154 std::cout <<
"finished." << std::endl;
158 std::cout << std::endl;
159 std::cout <<
"Set target position of a motor, freeze or " 160 <<
"switch motors off (C Command)" << std::endl;
163 std::cout <<
" motor number: ";
165 if ((param1 < 1) || (param1 > 6)) {
168 std::cout <<
" motor command flag: ";
170 std::cout <<
" targetposition: ";
175 packet[1] = (
byte) param1;
176 packet[2] = (
byte) param2;
177 packet[3] = (
byte) (param3 >> 8);
178 packet[4] = (
byte) param3;
181 protocol->comm(packet, buffer, &size);
182 if (!buffer[0] || ((
short) size != 3)) {
183 std::cout <<
" command failed!" << std::endl;
185 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
186 <<
"," << (short) buffer[1] <<
"," << (
short) buffer[2]
193 std::cout << std::endl;
194 std::cout <<
"Set encoder offset (C Command (motNr. + 128))" 198 std::cout <<
" motor number: ";
200 if ((param1 < 1) || (param1 > 6)) {
204 std::cout <<
" motor command flag: ";
206 std::cout <<
" offset position: ";
211 packet[1] = (
byte) param1;
212 packet[2] = (
byte) param2;
213 packet[3] = (
byte) (param3 >> 8);
214 packet[4] = (
byte) param3;
217 protocol->comm(packet, buffer, &size);
218 if (!buffer[0] || ((
short) size != 3)) {
219 std::cout <<
" command failed!" << std::endl;
221 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
222 <<
"," << (short) buffer[1] - 128 <<
"," << (
short) buffer[2]
229 std::cout << std::endl;
230 std::cout <<
"Read the position, velocity and pwm duty cycle " 231 <<
"of a motor (D Command)" << std::endl;
234 std::cout <<
" motor number: ";
236 if ((param1 < 1) || (param1 > 6)) {
242 packet[1] = (
byte) param1;
245 protocol->comm(packet, buffer, &size);
246 if (!buffer[0] || ((
short) size != 8)) {
247 std::cout <<
" command failed!" << std::endl;
249 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
250 <<
"," << (short) buffer[1] <<
"," << (
short) buffer[2]
251 <<
"," <<
b2s(buffer[3], buffer[4]) <<
"," 252 <<
b2s(buffer[5], buffer[6]) <<
"," 253 << (short) buffer[7] <<
"<" << std::endl;
259 std::cout << std::endl;
260 std::cout <<
"Set and read parameters and limits (S Command)" 264 std::cout <<
" motor number: ";
266 if ((param1 < 1) || (param1 > 6)) {
269 std::cout <<
" subcommand: ";
271 std::cout <<
" P1: ";
273 std::cout <<
" P2: ";
275 std::cout <<
" P3: ";
280 packet[1] = (
byte) param1;
281 packet[2] = (
byte) param2;
282 packet[3] = (
byte) param3;
283 packet[4] = (
byte) param4;
284 packet[5] = (
byte) param5;
287 protocol->comm(packet, buffer, &size);
288 if (!buffer[0] || ((
short) size != 6)) {
289 std::cout <<
" command failed!" << std::endl;
291 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
292 <<
"," << (short) buffer[1] <<
"," << (
short) buffer[2]
293 <<
"," << (short) buffer[3] <<
"," << (
short) buffer[4]
294 <<
"," << (short) buffer[5] <<
"<" << std::endl;
300 std::cout << std::endl;
301 std::cout <<
"Get all motors command flags or positions at " 302 <<
"once (N Command)" << std::endl;
305 std::cout <<
" subcommand: ";
307 std::cout <<
" subsubcommand: ";
312 packet[1] = (
byte) param1;
313 packet[2] = (
byte) param2;
316 protocol->comm(packet, buffer, &size);
317 if (!buffer[0] || ((
short) size < 2)) {
318 std::cout <<
" command failed!" << std::endl;
320 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0];
321 for (i = 1; i < (short) size; i++) {
322 std::cout <<
"," << (short) buffer[i];
324 std::cout <<
"<" << std::endl;
330 std::cout << std::endl;
331 std::cout <<
"Set the single polynomial movement parameters " 332 <<
"(G Command)" << std::endl;
335 std::cout <<
" motor number: ";
337 if ((param1 < 1) || (param1 > 6)) {
340 std::cout <<
" target pos: ";
342 std::cout <<
" time: ";
344 std::cout <<
" P10: ";
346 std::cout <<
" P11: ";
348 std::cout <<
" P12: ";
350 std::cout <<
" P13: ";
355 packet[1] = (
byte) param1;
356 packet[2] = (
byte) (param2 >> 8);
357 packet[3] = (
byte) param2;
358 packet[4] = (
byte) (param3 >> 8);
359 packet[5] = (
byte) param3;
360 packet[6] = (
byte) (param4 >> 8);
361 packet[7] = (
byte) param4;
362 packet[8] = (
byte) (param5 >> 8);
363 packet[9] = (
byte) param5;
364 packet[10] = (
byte) (param6 >> 8);
365 packet[11] = (
byte) param6;
366 packet[12] = (
byte) (param7 >> 8);
367 packet[13] = (
byte) param7;
370 protocol->comm(packet, buffer, &size);
371 if (!buffer[0] || ((
short) size != 2)) {
372 std::cout <<
" command failed!" << std::endl;
374 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
375 <<
"," << (short) buffer[1] <<
"<" << std::endl;
381 std::cout << std::endl;
382 std::cout <<
"Start the single polynomial movement " 383 <<
"(G+128 Command)" << std::endl;
386 std::cout <<
" subcommand: ";
388 std::cout <<
" exact flag: ";
392 packet[0] =
'G' | 0x80;
393 packet[1] = (
byte) param1;
394 packet[2] = (
byte) param2;
397 protocol->comm(packet, buffer, &size);
398 if (!buffer[0] || ((
short) size != 2)) {
399 std::cout <<
" command failed!" << std::endl;
401 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
402 <<
" (g+128)," << (short) buffer[1] <<
"<" 409 std::cout << std::endl;
410 std::cout <<
"Switch collision detection On and Off " 411 <<
"(A Command)" << std::endl;
414 std::cout <<
" master: ";
416 std::cout <<
" slave: ";
421 packet[1] = (
byte) param1;
422 packet[2] = (
byte) param2;
425 protocol->comm(packet, buffer, &size);
426 if (!buffer[0] || ((
short) size != 2)) {
427 std::cout <<
" command failed!" << std::endl;
429 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
430 <<
"," << (short) buffer[1] <<
"<" << std::endl;
436 std::cout << std::endl;
437 std::cout <<
"Read Echo (Z Command)" << std::endl;
443 protocol->comm(packet, buffer, &size);
444 if (!buffer[0] || ((
short) size != 1)) {
445 std::cout <<
" command failed!" << std::endl;
447 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
454 std::cout << std::endl;
455 std::cout <<
"Read the Identification String (Y Command)" 462 protocol->comm(packet, buffer, &size);
464 std::cout <<
" command failed!" << std::endl;
467 std::cout <<
" answer:" << std::endl <<
" >" << buffer
477 std::cout << std::endl;
478 std::cout <<
"Read the Katana Command Table (X Command)" 485 protocol->comm(packet, buffer, &size);
486 if (!buffer[0] || ((
short) size < 7) || (((
short) size)%6 != 1))
488 std::cout <<
" command failed!" << std::endl;
490 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
491 <<
"(command table)<" << std::endl <<
" command table:" 493 for (i = 0; i < (((short) size - 1) / 6); i++) {
494 std::cout <<
" " << (short) buffer[6*i+1] <<
",";
495 if ((
short) buffer[6*i+2] < 128) {
496 std::cout << buffer[6*i+2];
498 std::cout << (char) (buffer[6*i+2] & 0x7f)
501 std::cout <<
"," << (short) buffer[6*i+3] <<
"," 502 << (
short) buffer[6*i+4] <<
"," 503 << (short) buffer[6*i+5] <<
",";
504 if ((
short) buffer[6*i+6] < 128) {
505 std::cout << buffer[6*i+6];
507 std::cout << (char) (buffer[6*i+6] & 0x7f)
510 std::cout << std::endl;
517 std::cout << std::endl;
518 std::cout <<
"Read Gripper Sensor Data (E Command)" 522 std::cout <<
" sensor controller number: ";
530 packet[1] = (
byte) param1;
533 protocol->comm(packet, buffer, &size);
534 if (!buffer[0] || ((
short) size != 18)) {
535 std::cout <<
" command failed!" << std::endl;
537 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
538 <<
"," << (short) buffer[1];
539 for (i = 2; i < (short) size; i++) {
540 std::cout <<
"," << (short) buffer[i];
542 std::cout <<
"<" << std::endl;
548 std::cout << std::endl;
549 std::cout <<
"Read the Master firmware-version (B Command)" 556 protocol->comm(packet, buffer, &size);
557 if (!buffer[0] || ((
short) size != 3)) {
558 std::cout <<
" command failed!" << std::endl;
560 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
561 <<
"," << (short) buffer[1] <<
"," << (
short) buffer[2]
568 std::cout << std::endl;
569 std::cout <<
"Read the Slave firmware-version (V Command)" 573 std::cout <<
" motor number: ";
575 if ((param1 < 1) || (param1 > 6)) {
581 packet[1] = (
byte) param1;
585 protocol->comm(packet, buffer, &size);
586 if (!buffer[0] || ((
short) size != 13)) {
587 std::cout <<
" command failed!" << std::endl;
589 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0];
590 for (i = 1; i < (short) size; i++) {
591 std::cout <<
"," << (short) buffer[i];
593 std::cout <<
"<" << std::endl;
599 std::cout << std::endl;
600 std::cout <<
"Get motCommand from slave (I Command)" 604 std::cout <<
" motor number: ";
606 if ((param1 < 1) || (param1 > 6)) {
612 packet[1] = (
byte) param1;
615 protocol->comm(packet, buffer, &size);
616 if (!buffer[0] || ((
short) size != 3)) {
617 std::cout <<
" command failed!" << std::endl;
619 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
620 <<
"," << (short) buffer[1] <<
"," << (
short) buffer[2]
627 std::cout << std::endl;
628 std::cout <<
"Modbus read and writes (M Command)" << std::endl;
631 std::cout <<
" 'R'ead or 'W'rite: ";
633 if ((p1 !=
'R') && (p1 !=
'W')) {
636 std::cout <<
" register: ";
638 std::cout <<
" value: ";
643 packet[1] = (
byte) p1;
644 packet[2] = (
byte) param2;
645 packet[3] = (
byte) (param3 >> 8);
646 packet[4] = (
byte) param3;
649 protocol->comm(packet, buffer, &size);
650 if (!buffer[0] || ((
short) size != 4)) {
651 std::cout <<
" command failed!" << std::endl;
653 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
654 <<
"," <<
b2s(buffer[1], buffer[2]) <<
"," 655 << (short) buffer[3] <<
"<" << std::endl;
661 std::cout << std::endl;
662 std::cout <<
"Katana 1.2 I/O (T Command)" << std::endl;
665 std::cout <<
" read (r), write (w) or set LED (l): ";
667 if ((p1 !=
'r') && (p1 !=
'w') && (p1 !=
'l')) {
671 std::cout <<
" value to write: ";
673 }
else if (p1 ==
'l') {
674 std::cout <<
" set led to 'r'ed, 'g'reen or '0'(off): ";
676 if ((param2 ==
'r') || (param2 ==
'g')) {
687 packet[1] = (
byte) p1;
688 packet[2] = (
byte) param2;
693 protocol->comm(packet, buffer, &size);
694 if (!buffer[0] || ((
short) size != 2)) {
695 std::cout <<
" command failed!" << std::endl;
697 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
698 <<
"," << (short) buffer[1] <<
"<" << std::endl;
704 std::cout << std::endl;
705 std::cout <<
"Navigation Control (O Command)" 709 std::cout <<
" command: ";
711 if ((param1 < 0) || (param1 > 3)) {
720 packet[1] = (
byte) param1;
721 packet[2] = (
byte) param2;
724 protocol->comm(packet, buffer, &size);
725 if (!buffer[0] || ((
short) size != 3)) {
726 std::cout <<
" command failed!" << std::endl;
728 std::cout <<
" answer:" << std::endl <<
" >" << buffer[0]
729 <<
"," << (short) buffer[1] <<
"," << (
short) buffer[2]
736 std::cout <<
"\n'" << input <<
"' is not a valid command.\n" 742 std::cout <<
"\nERROR: " << e.
message() << std::endl;
static std::unique_ptr< CCdlSocket > device
unsigned char byte
type specification (8 bit)
int main(int argc, char *argv[])
static std::unique_ptr< CCplSerialCRC > protocol
std::string message() const
short b2s(byte hi, byte lo)
Implement the Serial-Zero protocol.
Encapsulates the socket communication device.
std::unique_ptr< CLMBase > katana