cplSerial.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 Neuronics AG
00004  *   Check out the AUTHORS file for detailed contact information.
00005  *
00006  *   This program is free software; you can redistribute it and/or modify
00007  *   it under the terms of the GNU General Public License as published by
00008  *   the Free Software Foundation; either version 2 of the License, or
00009  *   (at your option) any later version.
00010  *
00011  *   This program is distributed in the hope that it will be useful,
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *   GNU General Public License for more details.
00015  *
00016  *   You should have received a copy of the GNU General Public License
00017  *   along with this program; if not, write to the Free Software
00018  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 #include "KNI/cplSerial.h"
00022 #include "KNI/CRC.h"
00023 #include <assert.h>
00024 #include <iostream>
00025 //<tfromm date="22.05.2009">
00026 #include <cstring>
00027 //</tfromm>
00028 
00029 
00030 #include <stdio.h>
00031 
00032 
00033 bool CCplSerialCRC::load_tbl() {
00034 
00035         bool status = true;
00036 
00037         // set table to zero
00038         memset(cmd,0,sizeof(cmd));
00039 
00040         // set a tentative command table for the katana
00041         cmd[(int)'B'].send_sz   = 1;
00042         cmd[(int)'B'].read_sz   = 3;
00043         cmd[(int)'X'].send_sz   = 1;
00044         cmd[(int)'X'].read_sz   = 181;
00045         cmd[(int)'Y'].send_sz   = 1;
00046         cmd[(int)'Y'].read_sz   = 84;
00047         cmd[(int)'Z'].send_sz   = 1;
00048         cmd[(int)'Z'].read_sz   = 1;
00049         cmd[(int)'C'].send_sz   = 5;
00050         cmd[(int)'C'].read_sz   = 3;
00051         cmd[(int)'D'].send_sz   = 2;
00052         cmd[(int)'D'].read_sz   = 8;
00053         cmd[(int)'E'].send_sz   = 2;
00054         cmd[(int)'E'].read_sz   = 18;
00055         cmd[(int)'V'].send_sz   = 3;
00056         cmd[(int)'V'].read_sz   = 13;
00057         cmd[(int)'N'].send_sz   = 3;
00058         cmd[(int)'N'].read_sz   = 13;
00059         cmd[(int)'G'].send_sz   = 14;
00060         cmd[(int)'G'].read_sz   = 2;
00061         cmd[(int)'G'+128].send_sz       = 3;
00062         cmd[(int)'G'+128].read_sz       = 2;
00063         cmd[(int)'H'].send_sz   = 75;
00064         cmd[(int)'H'].read_sz   = 3;
00065         cmd[(int)'A'].send_sz   = 3;
00066         cmd[(int)'A'].read_sz   = 2;
00067         cmd[(int)'S'].send_sz   = 6;
00068         cmd[(int)'S'].read_sz   = 6;
00069         cmd[(int)'I'].send_sz   = 2;
00070         cmd[(int)'I'].read_sz   = 3;
00071         cmd[(int)'M'].send_sz   = 5;
00072         cmd[(int)'M'].read_sz   = 4;
00073         cmd[(int)'T'].send_sz   = 5;
00074         cmd[(int)'T'].read_sz   = 2;
00075 
00076         return status;
00077 }
00078 
00079 void CCplSerialCRC::defineProtocol(byte _kataddr) {
00080         hdr.size = 3;
00081         hdr.data[0] = 1;        //convention
00082         hdr.data[1] = _kataddr; //complete header
00083 }
00084 
00085 bool CCplSerialCRC::init(CCdlBase* _device, byte _kataddr) {
00086         device = _device;
00087         defineProtocol(_kataddr);
00088         return load_tbl();
00089 }
00090 
00091 
00092 void CCplSerialCRC::comm(const byte* pack, byte* buf, byte* size) {
00093         // This method enssamble the packet with the header, data, and CRC.
00094         // Sends it and receives the answer.
00095         memset(send_buf,0,256);                                                 //override old values
00096         hdr.data[hdr.size-1] = cmd[pack[0]].send_sz;    //complete header
00097         memcpy(send_buf, hdr.data, hdr.size);
00098         memcpy(send_buf+hdr.size,pack,hdr.data[hdr.size-1]);
00099 
00100         short crc   = CRC_CHECKSUM((uint8*)pack,hdr.data[hdr.size-1]);
00101         byte  bufsz = hdr.size + hdr.data[hdr.size-1];
00102         send_buf[bufsz++] = (byte)(crc >> 8);                   //hi-byte
00103         send_buf[bufsz++] = (byte)(crc & 0xFF);                 //lo-byte
00104 
00105         memset(read_buf,0,256);                                                 //read through device
00106         byte read_sz = cmd[pack[0]].read_sz + 2;
00107 
00108         short tries_recv = 0;
00109         while(true) {
00110 
00111 
00112                 try {
00113                         // uncomment to get debug output:
00114                         // if (pack[0] != 'N') {
00115                         //      // N = Get all axis's positions at once
00116                         //      printf("KNI >>> %c", pack[0]);
00117                         //      for (size_t i = 1; i < cmd[pack[0]].send_sz; i++)
00118                         //      printf(" %d", pack[i]);
00119                         //      printf("\n");
00120                         // }
00121                         send(send_buf, bufsz, NUMBER_OF_RETRIES_SEND); // pass exceptions from this since already retried
00122                 }
00123                 catch(...){
00124                         throw;
00125                 }
00126                 //For comm DEBUG only: std::cout << "send OK, try to receive...\n";
00127                 try {
00128                         recv(read_buf,read_sz,size);
00129 
00130                         // uncomment to get debug output:
00131                         // if (read_buf[0] != 'n')
00132                         // {
00133                         //      printf("KNI <<< %c", read_buf[0]);
00134                         //      for (size_t i = 1; i < *size; i++)
00135                         //      printf(" %d", read_buf[i]);
00136                         //      printf("\n");
00137                         // }
00138                         memcpy(buf,read_buf,*size); // copy read_buf to _buf
00139                 } catch ( ReadNotCompleteException & ) {
00140                         if(tries_recv < NUMBER_OF_RETRIES_RECV) {
00141                                 ++tries_recv;
00142                                 continue;
00143                         }
00144                         throw;
00145 
00146                 } catch ( WrongCRCException & ) {
00147                         if(tries_recv < NUMBER_OF_RETRIES_RECV) {
00148                                 ++tries_recv;
00149                                 continue;
00150                         } else {
00151                                 throw;
00152                         }
00153                 } catch ( FirmwareException & ) {
00154                         throw;
00155                 } catch ( Exception & ) {
00156                         throw;
00157                         // FIXME why? --MRE
00158 //                      if(errorFlag == true){
00159 //                      }
00160                 } catch(...){
00161                         throw;
00162                 }
00163                 break; // write succeeded
00164 
00165         }
00166 }
00167 
00168 void CCplSerialCRC::getMasterFirmware(short* fw, short* rev) {
00169         *fw = mMasterVersion;
00170         *rev = mMasterRevision;
00171 }
00172 
00173 void CCplSerialCRC::send(byte* send_buf, byte bufsz, short retries) {
00174         short r = 0;
00175         while(true) {
00176                 try {
00177                         device->send(send_buf, bufsz); //send to device
00178                 } catch ( WriteNotCompleteException & ) {
00179                         if(r < retries) {
00180                                 ++r;
00181                                 continue;
00182                         } else {
00183                                 throw;
00184                         }
00185                 } catch ( Exception & ) {
00186                         throw; // throw other exceptions immediately
00187                 }
00188                 break;
00189         }
00190 }
00191 
00192 void CCplSerialCRC::recv(byte* read_buf, byte read_sz, byte* size) {
00193 
00194         // Receives the packet and checks the CRC.
00195         *size = device->recv(read_buf, read_sz);                //receives from device
00196 
00197         bool getErrorMessage = false;
00198         //check for error from Katana:
00199         if(read_buf[0] == KATANA_ERROR_FLAG){
00200                 std::cout << "Error flag received:\n";
00201                 assert(*size == 3);
00202                 getErrorMessage = true;
00203                 read_sz = *size; // FIXME: should not return the now invalid buffer?
00204         } else {
00205                 if (*size != read_sz) {
00206                         throw ReadNotCompleteException("?"); // FIXME: should get _ipAddr for nicer error message
00207                 }
00208         }
00209 
00210         *size -= 2;
00211         byte bhi = read_buf[read_sz-2];
00212         byte blo = read_buf[read_sz-1];
00213 
00214         short crc = CRC_CHECKSUM((uint8*)read_buf,*size);
00215         byte hi = (byte)(crc >> 8);
00216         byte lo = (byte)(crc & 0xFF);
00217 
00218         if ((hi != bhi) || (lo != blo)) {
00219                 std::cout << "warning: crc error, throwing exception" << std::endl;
00220                 throw WrongCRCException();
00221         }
00222 
00223         if (getErrorMessage) {
00224                 byte buf[57];
00225                 buf[0] = 0; // ignored
00226                 buf[1] = 0; // ignored
00227                 buf[2] = 0; // ignored
00228                 buf[3] = KATANA_ERROR_FLAG+1;
00229                 try {
00230                         send(buf, 4, 1);
00231                         byte size_errormsg = 57;
00232                         recv(buf, 57, &size_errormsg);
00233                 } catch (...) {
00234                         std::cout << "Error while requesting error message!\n";
00235                 }
00236 
00237                 if (buf[0] != KATANA_ERROR_FLAG+1) {
00238                         std::cout << "bad response to error request\n";
00239                 }
00240                 byte lastCommand = buf[1];
00241                 byte errorCode = buf[2];
00242                 byte device = buf[3];
00243                 std::string errorString((char*)buf+4);
00244                 if (device != 0) {
00245                         errorString += " (axis ";
00246                         errorString += '0' + device;
00247                         errorString += ")";
00248                 }
00249                 //std::cout << "\"" << errorString << "\"\n";
00250                 throw FirmwareException(errorString, static_cast<signed char>(errorCode), device, lastCommand);
00251         }
00252 }


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