commands.cpp
Go to the documentation of this file.
00001 /**********************************************************************************
00002  *   Katana Native Interface - A C++ interface to the robot arm Katana.
00003  *   Copyright (C) 2005-2008 Neuronics AG
00004  *   Check out the AUTHORS file for detailed contact information.
00005  *   This program is free software; you can redistribute it and/or modify
00006  *   it under the terms of the GNU General Public License as published by
00007  *   the Free Software Foundation; either version 2 of the License, or
00008  *   (at your option) any later version.
00009  *   This program is distributed in the hope that it will be useful,
00010  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *   GNU General Public License for more details.
00013  *   You should have received a copy of the GNU General Public License
00014  *   along with this program; if not, write to the Free Software
00015  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00016  **********************************************************************************/
00018 // commands.cpp
00019 // demo program for KNI libraries
00021 #include "kniBase.h"
00022 #include "../../include/KNI/kmlMotBase.h"
00023 #include <iostream>
00024 #include <cstdio>
00025 #include <memory>
00027 #ifdef WIN32
00028 #       include <conio.h>
00029 #else //LINUX
00030 #       include "keyboard.h"
00031 #endif
00032 
00033 //Katana object
00034 std::auto_ptr<CLMBase> katana;
00036 void DisplayHelp() {
00037         std::cout << std::endl;
00038         std::cout << "-------------------------------------------" << std::endl;
00039         std::cout << "Esc: quit program" << std::endl;
00040         std::cout << "?: Display this help" << std::endl;
00041         std::cout << "j: Calibrate the Katana" << std::endl;
00042         std::cout << "c: Set target position, freeze or switch motor off (C Command)" << std::endl;
00043         std::cout << "C: Set encoder offset (C Command (motNr. + 128))" << std::endl;
00044         std::cout << "d: Read the position, velocity and pwm duty cycle of a motor (D Command)" << std::endl;
00045         std::cout << "s: Set and read parameters and limits (S Command)" << std::endl;
00046         std::cout << "n: Get all motors command flags or positions at once (N Command)" << std::endl;
00047         std::cout << "g: Set the single polynomial movement parameters (G Command)" << std::endl;
00048         std::cout << "G: Start the single polynomial movement (G+128 Command)" << std::endl;
00049         std::cout << "a: Switch collision detection On and Off (A Command)" << std::endl;
00050         std::cout << "z: Read Echo (Z Command)" << std::endl;
00051         std::cout << "y: Read the Identification String (Y Command)" << std::endl;
00052         std::cout << "x: Read the Katana Command Table (X Command)" << std::endl;
00053         std::cout << "e: Read Gripper Sensor Data (E Command)" << std::endl;
00054         std::cout << "b: Read the Master firmware-version (B Command)" << std::endl;
00055         std::cout << "v: Read the Slave firmware-version (V Command)" << std::endl;
00056         std::cout << "i: Get motCommand from slave (I Command)" << std::endl;
00057         std::cout << "m: Modbus read and write (M Command)" << std::endl;
00058         std::cout << "t: Katana 1.2 I/O (T Command)" << std::endl;
00059         std::cout << "o: Navigation Control (O Command)" << std::endl;
00060         std::cout << "-------------------------------------------" << std::endl;
00061 }
00063 // convert two bytes (hi and lo) to short
00064 short b2s(byte hi, byte lo) {
00065         return (((short) hi) << 8) + ((short) lo);
00066 }
00068 int main(int argc, char *argv[]) {
00069 
00070         if (argc != 3) {
00071                 std::cout << "usage for socketcommands: socketcommands CONFIGFILE"
00072                         << " IP_ADDR" << std::endl;
00073                 return 0;
00074         }
00075 
00076         std::cout << "---------------------------" << std::endl;
00077         std::cout << "SOCKETCOMMANDS DEMO STARTED" << std::endl;
00078         std::cout << "---------------------------" << std::endl;
00079 
00080         //----------------------------------------------------------------//
00081         //open device: a serial port is opened in this case
00082         //----------------------------------------------------------------//
00083 
00084                 
00085         std::auto_ptr<CCdlSocket> device;
00086         std::auto_ptr<CCplSerialCRC> protocol;
00087 
00088         try {
00089 
00090                 int port = 5566;
00091                 device.reset(new CCdlSocket(argv[2], port));
00092                 
00093                 std::cout << "-------------------------------------------" << std::endl;
00094                 std::cout << "success:  port " << port << " open" << std::endl;
00095                 std::cout << "-------------------------------------------" << std::endl;
00096 
00097                 //--------------------------------------------------------//
00098                 //init protocol:
00099                 //--------------------------------------------------------//
00100 
00101                 protocol.reset(new CCplSerialCRC());
00102                 protocol->init(device.get());
00103                 std::cout << "-------------------------------------------" << std::endl;
00104                 std::cout << "success: protocol initiated" << std::endl;
00105                 std::cout << "-------------------------------------------" << std::endl;
00106 
00107 
00108                 //--------------------------------------------------------//
00109                 //init robot:
00110                 //--------------------------------------------------------//
00111 
00112                 katana.reset(new CLMBase());
00113                 katana->create(argv[1], protocol.get());
00114 
00115 
00116         } catch(Exception &e) {
00117                 std::cout << "ERROR: " << e.message() << std::endl;
00118                 return -1;
00119         }
00120         std::cout << "-------------------------------------------" << std::endl;
00121         std::cout << "success: katana initiated" << std::endl;
00122         std::cout << "-------------------------------------------" << std::endl;
00123 
00124         DisplayHelp();
00125         
00126         // declare variables used in loop
00127         byte    packet[32]; //packet
00128         byte    buffer[256]; //readbuf
00129         byte    size = 0; //readbuf size
00130         short param1, param2, param3, param4, param5, param6, param7;
00131         char p1, p2;
00132         int i;
00133 
00134         bool loop = true;
00135 
00136         while (loop) {
00137 
00138                 int input = _getch();
00139 
00140                 try {
00141                         switch (input) {
00142                         case 27: //VK_ESCAPE
00143                                 loop = false;
00144                                 break;
00145 
00146                         case '?':
00147                                 DisplayHelp();
00148                                 break;
00149 
00150                         case 'j': //VK_J (Calibration)
00151                                 std::cout << std::endl;
00152                                 std::cout << "Calibrating Katana... ";
00153                                 katana->calibrate();
00154                                 std::cout << "finished." << std::endl;
00155                                 break;
00156 
00157                         case 'c': //VK_C (C Command)
00158                                 std::cout << std::endl;
00159                                 std::cout << "Set target position of a motor, freeze or "
00160                                         << "switch motors off (C Command)" << std::endl;
00161 
00162                                 // get parameters
00163                                 std::cout << " motor number: ";
00164                                 std::cin >> param1;
00165                                 if ((param1 < 1) || (param1 > 6)) {
00166                                         break;
00167                                 }
00168                                 std::cout << " motor command flag: ";
00169                                 std::cin >> param2;
00170                                 std::cout << " targetposition: ";
00171                                 std::cin >> param3;
00172 
00173                                 // create packet
00174                                 packet[0] = 'C';
00175                                 packet[1] = (byte) param1;
00176                                 packet[2] = (byte) param2;
00177                                 packet[3] = (byte) (param3 >> 8);
00178                                 packet[4] = (byte) param3;
00179 
00180                                 // communicate packet
00181                                 protocol->comm(packet, buffer, &size);
00182                                 if (!buffer[0] || ((short) size != 3)) {
00183                                         std::cout << " command failed!" << std::endl;
00184                                 } else {
00185                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00186                                                 << "," << (short) buffer[1] << "," << (short) buffer[2]
00187                                                 << "<" << std::endl;
00188                                 }
00189                                 buffer[0] = 0x00;
00190                                 break;
00191 
00192                         case 'C': //VK_C (C Command (motNr. + 128))
00193                                 std::cout << std::endl;
00194                                 std::cout << "Set encoder offset (C Command (motNr. + 128))"
00195                                         << std::endl;
00196 
00197                                 // get parameters
00198                                 std::cout << " motor number: ";
00199                                 std::cin >> param1;
00200                                 if ((param1 < 1) || (param1 > 6)) {
00201                                         break;
00202                                 }
00203                                 param1 += 128;
00204                                 std::cout << " motor command flag: ";
00205                                 std::cin >> param2;
00206                                 std::cout << " offset position: ";
00207                                 std::cin >> param3;
00208 
00209                                 // create packet
00210                                 packet[0] = 'C';
00211                                 packet[1] = (byte) param1;
00212                                 packet[2] = (byte) param2;
00213                                 packet[3] = (byte) (param3 >> 8);
00214                                 packet[4] = (byte) param3;
00215 
00216                                 // communicate packet
00217                                 protocol->comm(packet, buffer, &size);
00218                                 if (!buffer[0] || ((short) size != 3)) {
00219                                         std::cout << " command failed!" << std::endl;
00220                                 } else {
00221                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00222                                                 << "," << (short) buffer[1] - 128 << "," << (short) buffer[2]
00223                                                 << "<" << std::endl;
00224                                 }
00225                                 buffer[0] = 0x00;
00226                                 break;
00227 
00228                         case 'd': //VK_D (D Command)
00229                                 std::cout << std::endl;
00230                                 std::cout << "Read the position, velocity and pwm duty cycle "
00231                                         << "of a motor (D Command)" << std::endl;
00232 
00233                                 // get parameters
00234                                 std::cout << " motor number: ";
00235                                 std::cin >> param1;
00236                                 if ((param1 < 1) || (param1 > 6)) {
00237                                         break;
00238                                 }
00239 
00240                                 // create packet
00241                                 packet[0] = 'D';
00242                                 packet[1] = (byte) param1;
00243 
00244                                 // communicate packet
00245                                 protocol->comm(packet, buffer, &size);
00246                                 if (!buffer[0] || ((short) size != 8)) {
00247                                         std::cout << " command failed!" << std::endl;
00248                                 } else {
00249                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00250                                                 << "," << (short) buffer[1] << "," << (short) buffer[2]
00251                                                 << "," << b2s(buffer[3], buffer[4]) << ","
00252                                                 << b2s(buffer[5], buffer[6]) << ","
00253                                                 << (short) buffer[7] << "<" << std::endl;
00254                                 }
00255                                 buffer[0] = 0x00;
00256                                 break;
00257 
00258                         case 's': //VK_S (S Command)
00259                                 std::cout << std::endl;
00260                                 std::cout << "Set and read parameters and limits (S Command)"
00261                                         << std::endl;
00262 
00263                                 // get parameters
00264                                 std::cout << " motor number: ";
00265                                 std::cin >> param1;
00266                                 if ((param1 < 1) || (param1 > 6)) {
00267                                         break;
00268                                 }
00269                                 std::cout << " subcommand: ";
00270                                 std::cin >> param2;
00271                                 std::cout << " P1: ";
00272                                 std::cin >> param3;
00273                                 std::cout << " P2: ";
00274                                 std::cin >> param4;
00275                                 std::cout << " P3: ";
00276                                 std::cin >> param5;
00277 
00278                                 // create packet
00279                                 packet[0] = 'S';
00280                                 packet[1] = (byte) param1;
00281                                 packet[2] = (byte) param2;
00282                                 packet[3] = (byte) param3;
00283                                 packet[4] = (byte) param4;
00284                                 packet[5] = (byte) param5;
00285 
00286                                 // communicate packet
00287                                 protocol->comm(packet, buffer, &size);
00288                                 if (!buffer[0] || ((short) size != 6)) {
00289                                         std::cout << " command failed!" << std::endl;
00290                                 } else {
00291                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00292                                                 << "," << (short) buffer[1] << "," << (short) buffer[2]
00293                                                 << "," << (short) buffer[3] << "," << (short) buffer[4]
00294                                                 << "," << (short) buffer[5] << "<" << std::endl;
00295                                 }
00296                                 buffer[0] = 0x00;
00297                                 break;
00298 
00299                         case 'n': //VK_N (N Command)
00300                                 std::cout << std::endl;
00301                                 std::cout << "Get all motors command flags or positions at "
00302                                         << "once (N Command)" << std::endl;
00303 
00304                                 // get parameters
00305                                 std::cout << " subcommand: ";
00306                                 std::cin >> param1;
00307                                 std::cout << " subsubcommand: ";
00308                                 std::cin >> param2;
00309 
00310                                 // create packet
00311                                 packet[0] = 'N';
00312                                 packet[1] = (byte) param1;
00313                                 packet[2] = (byte) param2;
00314 
00315                                 // communicate packet
00316                                 protocol->comm(packet, buffer, &size);
00317                                 if (!buffer[0] || ((short) size < 2)) {
00318                                         std::cout << " command failed!" << std::endl;
00319                                 } else {
00320                                         std::cout << " answer:" << std::endl << "  >" << buffer[0];
00321                                         for (i = 1; i < (short) size; i++) {
00322                                                 std::cout << "," << (short) buffer[i];
00323                                         }
00324                                         std::cout << "<" << std::endl;
00325                                 }
00326                                 buffer[0] = 0x00;
00327                                 break;
00328 
00329                         case 'g': //VK_G (G Command)
00330                                 std::cout << std::endl;
00331                                 std::cout << "Set the single polynomial movement parameters "
00332                                         << "(G Command)" << std::endl;
00333 
00334                                 // get parameters
00335                                 std::cout << " motor number: ";
00336                                 std::cin >> param1;
00337                                 if ((param1 < 1) || (param1 > 6)) {
00338                                         break;
00339                                 }
00340                                 std::cout << " target pos: ";
00341                                 std::cin >> param2;
00342                                 std::cout << " time: ";
00343                                 std::cin >> param3;
00344                                 std::cout << " P10: ";
00345                                 std::cin >> param4;
00346                                 std::cout << " P11: ";
00347                                 std::cin >> param5;
00348                                 std::cout << " P12: ";
00349                                 std::cin >> param6;
00350                                 std::cout << " P13: ";
00351                                 std::cin >> param7;
00352 
00353                                 // create packet
00354                                 packet[0] = 'G';
00355                                 packet[1] = (byte) param1;
00356                                 packet[2] = (byte) (param2 >> 8);
00357                                 packet[3] = (byte) param2;
00358                                 packet[4] = (byte) (param3 >> 8);
00359                                 packet[5] = (byte) param3;
00360                                 packet[6] = (byte) (param4 >> 8);
00361                                 packet[7] = (byte) param4;
00362                                 packet[8] = (byte) (param5 >> 8);
00363                                 packet[9] = (byte) param5;
00364                                 packet[10] = (byte) (param6 >> 8);
00365                                 packet[11] = (byte) param6;
00366                                 packet[12] = (byte) (param7 >> 8);
00367                                 packet[13] = (byte) param7;
00368 
00369                                 // communicate packet
00370                                 protocol->comm(packet, buffer, &size);
00371                                 if (!buffer[0] || ((short) size != 2)) {
00372                                         std::cout << " command failed!" << std::endl;
00373                                 } else {
00374                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00375                                                 << "," << (short) buffer[1] << "<" << std::endl;
00376                                 }
00377                                 buffer[0] = 0x00;
00378                                 break;
00379 
00380                         case 'G': //VK_G (G+128 Command)
00381                                 std::cout << std::endl;
00382                                 std::cout << "Start the single polynomial movement "
00383                                         << "(G+128 Command)" << std::endl;
00384 
00385                                 // get parameters
00386                                 std::cout << " subcommand: ";
00387                                 std::cin >> param1;
00388                                 std::cout << " exact flag: ";
00389                                 std::cin >> param2;
00390 
00391                                 // create packet
00392                                 packet[0] = 'G' | 0x80;
00393                                 packet[1] = (byte) param1;
00394                                 packet[2] = (byte) param2;
00395 
00396                                 // communicate packet
00397                                 protocol->comm(packet, buffer, &size);
00398                                 if (!buffer[0] || ((short) size != 2)) {
00399                                         std::cout << " command failed!" << std::endl;
00400                                 } else {
00401                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00402                                                 << " (g+128)," << (short) buffer[1] << "<"
00403                                                 << std::endl;
00404                                 }
00405                                 buffer[0] = 0x00;
00406                                 break;
00407 
00408                         case 'a': //VK_A (A Command)
00409                                 std::cout << std::endl;
00410                                 std::cout << "Switch collision detection On and Off "
00411                                         << "(A Command)" << std::endl;
00412 
00413                                 // get parameters
00414                                 std::cout << " master: ";
00415                                 std::cin >> param1;
00416                                 std::cout << " slave: ";
00417                                 std::cin >> param2;
00418 
00419                                 // create packet
00420                                 packet[0] = 'A';
00421                                 packet[1] = (byte) param1;
00422                                 packet[2] = (byte) param2;
00423 
00424                                 // communicate packet
00425                                 protocol->comm(packet, buffer, &size);
00426                                 if (!buffer[0] || ((short) size != 2)) {
00427                                         std::cout << " command failed!" << std::endl;
00428                                 } else {
00429                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00430                                                 << "," << (short) buffer[1] << "<" << std::endl;
00431                                 }
00432                                 buffer[0] = 0x00;
00433                                 break;
00434 
00435                         case 'z': //VK_Z (Z Command)
00436                                 std::cout << std::endl;
00437                                 std::cout << "Read Echo (Z Command)" << std::endl;
00438 
00439                                 // create packet
00440                                 packet[0] = 'Z';
00441 
00442                                 // communicate packet
00443                                 protocol->comm(packet, buffer, &size);
00444                                 if (!buffer[0] || ((short) size != 1)) {
00445                                         std::cout << " command failed!" << std::endl;
00446                                 } else {
00447                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00448                                                 << "<" << std::endl;
00449                                 }
00450                                 buffer[0] = 0x00;
00451                                 break;
00452 
00453                         case 'y': //VK_Y (Y Command)
00454                                 std::cout << std::endl;
00455                                 std::cout << "Read the Identification String (Y Command)"
00456                                         << std::endl;
00457 
00458                                 // create packet
00459                                 packet[0] = 'Y';
00460 
00461                                 // communicate packet
00462                                 protocol->comm(packet, buffer, &size);
00463                                 if (!buffer[0]) {
00464                                         std::cout << " command failed!" << std::endl;
00465                                 } else {
00466                                         buffer[82] = 0; // terminate c-string before \r\n
00467                                         std::cout << " answer:" << std::endl << "  >" << buffer
00468                                                 << "<" << std::endl;
00469                                 }
00470                                 buffer[0] = 0x00;
00471                                 break;
00472 
00473                                 buffer[0] = 0x00;
00474                                 break;
00475 
00476                         case 'x': //VK_X (X Command)
00477                                 std::cout << std::endl;
00478                                 std::cout << "Read the Katana Command Table (X Command)"
00479                                         << std::endl;
00480 
00481                                 // create packet
00482                                 packet[0] = 'X';
00483 
00484                                 // communicate packet
00485                                 protocol->comm(packet, buffer, &size);
00486                                 if (!buffer[0] || ((short) size < 7) || (((short) size)%6 != 1))
00487                                                 {
00488                                         std::cout << " command failed!" << std::endl;
00489                                 } else {
00490                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00491                                                 << "(command table)<" << std::endl << " command table:"
00492                                                 << std::endl;
00493                                         for (i = 0; i < (((short) size - 1) / 6); i++) {
00494                                                 std::cout << "  " << (short) buffer[6*i+1] << ",";
00495                                                 if ((short) buffer[6*i+2] < 128) {
00496                                                         std::cout << buffer[6*i+2];
00497                                                 } else {
00498                                                         std::cout << (char) (buffer[6*i+2] & 0x7f)
00499                                                                 << "+128";
00500                                                 }
00501                                                 std::cout << "," << (short) buffer[6*i+3] << ","
00502                                                         << (short) buffer[6*i+4] << ","
00503                                                         << (short) buffer[6*i+5] << ",";
00504                                                 if ((short) buffer[6*i+6] < 128) {
00505                                                         std::cout << buffer[6*i+6];
00506                                                 } else {
00507                                                         std::cout << (char) (buffer[6*i+6] & 0x7f)
00508                                                                 << "+128";
00509                                                 }
00510                                                 std::cout << std::endl;
00511                                         }
00512                                 }
00513                                 buffer[0] = 0x00;
00514                                 break;
00515 
00516                         case 'e': //VK_E (E Command)
00517                                 std::cout << std::endl;
00518                                 std::cout << "Read Gripper Sensor Data (E Command)"
00519                                         << std::endl;
00520 
00521                                 // get parameters
00522                                 std::cout << " sensor controller number: ";
00523                                 std::cin >> param1;
00524                                 if (param1 != 15) {
00525                                         break;
00526                                 }
00527 
00528                                 // create packet
00529                                 packet[0] = 'E';
00530                                 packet[1] = (byte) param1;
00531 
00532                                 // communicate packet
00533                                 protocol->comm(packet, buffer, &size);
00534                                 if (!buffer[0] || ((short) size != 18)) {
00535                                         std::cout << " command failed!" << std::endl;
00536                                 } else {
00537                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00538                                                 << "," << (short) buffer[1];
00539                                         for (i = 2; i < (short) size; i++) {
00540                                                 std::cout << "," << (short) buffer[i];
00541                                         }
00542                                         std::cout << "<" << std::endl;
00543                                 }
00544                                 buffer[0] = 0x00;
00545                                 break;
00546 
00547                         case 'b': //VK_B (B Command)
00548                                 std::cout << std::endl;
00549                                 std::cout << "Read the Master firmware-version (B Command)"
00550                                         << std::endl;
00551 
00552                                 // create packet
00553                                 packet[0] = 'B';
00554 
00555                                 // communicate packet
00556                                 protocol->comm(packet, buffer, &size);
00557                                 if (!buffer[0] || ((short) size != 3)) {
00558                                         std::cout << " command failed!" << std::endl;
00559                                 } else {
00560                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00561                                                 << "," << (short) buffer[1] << "," << (short) buffer[2]
00562                                                 << "<" << std::endl;
00563                                 }
00564                                 buffer[0] = 0x00;
00565                                 break;
00566 
00567                         case 'v': //VK_V (V Command)
00568                                 std::cout << std::endl;
00569                                 std::cout << "Read the Slave firmware-version (V Command)"
00570                                         << std::endl;
00571 
00572                                 // get parameters
00573                                 std::cout << " motor number: ";
00574                                 std::cin >> param1;
00575                                 if ((param1 < 1) || (param1 > 6)) {
00576                                         break;
00577                                 }
00578 
00579                                 // create packet
00580                                 packet[0] = 'V';
00581                                 packet[1] = (byte) param1;
00582                                 packet[2] = 32;
00583 
00584                                 // communicate packet
00585                                 protocol->comm(packet, buffer, &size);
00586                                 if (!buffer[0] || ((short) size != 13)) {
00587                                         std::cout << " command failed!" << std::endl;
00588                                 } else {
00589                                         std::cout << " answer:" << std::endl << "  >" << buffer[0];
00590                                         for (i = 1; i < (short) size; i++) {
00591                                                 std::cout << "," << (short) buffer[i];
00592                                         }
00593                                         std::cout << "<" << std::endl;
00594                                 }
00595                                 buffer[0] = 0x00;
00596                                 break;
00597 
00598                         case 'i': //VK_I (I Command)
00599                                 std::cout << std::endl;
00600                                 std::cout << "Get motCommand from slave (I Command)"
00601                                         << std::endl;
00602 
00603                                 // get parameters
00604                                 std::cout << " motor number: ";
00605                                 std::cin >> param1;
00606                                 if ((param1 < 1) || (param1 > 6)) {
00607                                         break;
00608                                 }
00609 
00610                                 // create packet
00611                                 packet[0] = 'I';
00612                                 packet[1] = (byte) param1;
00613 
00614                                 // communicate packet
00615                                 protocol->comm(packet, buffer, &size);
00616                                 if (!buffer[0] || ((short) size != 3)) {
00617                                         std::cout << " command failed!" << std::endl;
00618                                 } else {
00619                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00620                                                 << "," << (short) buffer[1] << "," << (short) buffer[2]
00621                                                 << "<" << std::endl;
00622                                 }
00623                                 buffer[0] = 0x00;
00624                                 break;
00625 
00626                         case 'm': //VK_M (M Command)
00627                                 std::cout << std::endl;
00628                                 std::cout << "Modbus read and writes (M Command)" << std::endl;
00629 
00630                                 // get parameters
00631                                 std::cout << " 'R'ead or 'W'rite: ";
00632                                 std::cin >> p1;
00633                                 if ((p1 != 'R') && (p1 != 'W')) {
00634                                         break;
00635                                 }
00636                                 std::cout << " register: ";
00637                                 std::cin >> param2;
00638                                 std::cout << " value: ";
00639                                 std::cin >> param3;
00640 
00641                                 // create packet
00642                                 packet[0] = 'M';
00643                                 packet[1] = (byte) p1;
00644                                 packet[2] = (byte) param2;
00645                                 packet[3] = (byte) (param3 >> 8);
00646                                 packet[4] = (byte) param3;
00647 
00648                                 // communicate packet
00649                                 protocol->comm(packet, buffer, &size);
00650                                 if (!buffer[0] || ((short) size != 4)) {
00651                                         std::cout << " command failed!" << std::endl;
00652                                 } else {
00653                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00654                                                 << "," << b2s(buffer[1], buffer[2]) << ","
00655                                                 << (short) buffer[3] << "<" << std::endl;
00656                                 }
00657                                 buffer[0] = 0x00;
00658                                 break;
00659 
00660                         case 't': //VK_T (T Command)
00661                                 std::cout << std::endl;
00662                                 std::cout << "Katana 1.2 I/O (T Command)" << std::endl;
00663 
00664                                 // get parameters
00665                                 std::cout << " read (r), write (w) or set LED (l): ";
00666                                 std::cin >> p1;
00667                                 if ((p1 != 'r') && (p1 != 'w') && (p1 != 'l')) {
00668                                         break;
00669                                 }
00670                                 if (p1 == 'w') {
00671                                         std::cout << " value to write: ";
00672                                         std::cin >> param2;
00673                                 } else if (p1 == 'l') {
00674                                         std::cout << " set led to 'r'ed, 'g'reen or '0'(off): ";
00675                                         std::cin >> p2;
00676                                         if ((param2 == 'r') || (param2 == 'g')) {
00677                                                 param2 = (short) p2;
00678                                         } else {
00679                                                 param2 = 0;
00680                                         }
00681                                 } else {
00682                                         param2 = 0;
00683                                 }
00684 
00685                                 // create packet
00686                                 packet[0] = 'T';
00687                                 packet[1] = (byte) p1;
00688                                 packet[2] = (byte) param2;
00689                                 packet[3] = 0;
00690                                 packet[4] = 0;
00691 
00692                                 // communicate packet
00693                                 protocol->comm(packet, buffer, &size);
00694                                 if (!buffer[0] || ((short) size != 2)) {
00695                                         std::cout << " command failed!" << std::endl;
00696                                 } else {
00697                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00698                                                 << "," << (short) buffer[1] << "<" << std::endl;
00699                                 }
00700                                 buffer[0] = 0x00;
00701                                 break;
00702 
00703                         case 'o': //VK_O (O Command)
00704                                 std::cout << std::endl;
00705                                 std::cout << "Navigation Control (O Command)"
00706                                         << std::endl;
00707 
00708                                 // get parameters
00709                                 std::cout << " command: ";
00710                                 std::cin >> param1;
00711                                 if ((param1 < 0) || (param1 > 3)) {
00712                                         break;
00713                                 }
00714                                 //std::cout << " CVar1: ";
00715                                 //std::cin >> param2;
00716                                 param2 = 0;
00717 
00718                                 // create packet
00719                                 packet[0] = 'O';
00720                                 packet[1] = (byte) param1;
00721                                 packet[2] = (byte) param2;
00722 
00723                                 // communicate packet
00724                                 protocol->comm(packet, buffer, &size);
00725                                 if (!buffer[0] || ((short) size != 3)) {
00726                                         std::cout << " command failed!" << std::endl;
00727                                 } else {
00728                                         std::cout << " answer:" << std::endl << "  >" << buffer[0]
00729                                                 << "," << (short) buffer[1] << "," << (short) buffer[2]
00730                                                 << "<" << std::endl;
00731                                 }
00732                                 buffer[0] = 0x00;
00733                                 break;
00734 
00735                         default: //Error message
00736                                 std::cout << "\n'" << input << "' is not a valid command.\n"
00737                                         << std::endl;
00738                                 break;
00739                         }
00740 
00741                 } catch (Exception &e) {
00742                         std::cout << "\nERROR: " << e.message() << std::endl;
00743                 }
00744 
00745         }
00746         return 0;
00747 }


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32