$search
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 }