cplSerial.cpp
Go to the documentation of this file.
1 /*
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
21 #include "KNI/cplSerial.h"
22 #include "KNI/CRC.h"
23 #include <assert.h>
24 #include <iostream>
25 //<tfromm date="22.05.2009">
26 #include <cstring>
27 //</tfromm>
28 
29 
30 #include <stdio.h>
31 
32 
34 
35  bool status = true;
36 
37  // set table to zero
38  memset(cmd,0,sizeof(cmd));
39 
40  // set a tentative command table for the katana
41  cmd[(int)'B'].send_sz = 1;
42  cmd[(int)'B'].read_sz = 3;
43  cmd[(int)'X'].send_sz = 1;
44  cmd[(int)'X'].read_sz = 181;
45  cmd[(int)'Y'].send_sz = 1;
46  cmd[(int)'Y'].read_sz = 84;
47  cmd[(int)'Z'].send_sz = 1;
48  cmd[(int)'Z'].read_sz = 1;
49  cmd[(int)'C'].send_sz = 5;
50  cmd[(int)'C'].read_sz = 3;
51  cmd[(int)'D'].send_sz = 2;
52  cmd[(int)'D'].read_sz = 8;
53  cmd[(int)'E'].send_sz = 2;
54  cmd[(int)'E'].read_sz = 18;
55  cmd[(int)'V'].send_sz = 3;
56  cmd[(int)'V'].read_sz = 13;
57  cmd[(int)'N'].send_sz = 3;
58  cmd[(int)'N'].read_sz = 13;
59  cmd[(int)'G'].send_sz = 14;
60  cmd[(int)'G'].read_sz = 2;
61  cmd[(int)'G'+128].send_sz = 3;
62  cmd[(int)'G'+128].read_sz = 2;
63  cmd[(int)'H'].send_sz = 75;
64  cmd[(int)'H'].read_sz = 3;
65  cmd[(int)'A'].send_sz = 3;
66  cmd[(int)'A'].read_sz = 2;
67  cmd[(int)'S'].send_sz = 6;
68  cmd[(int)'S'].read_sz = 6;
69  cmd[(int)'I'].send_sz = 2;
70  cmd[(int)'I'].read_sz = 3;
71  cmd[(int)'M'].send_sz = 5;
72  cmd[(int)'M'].read_sz = 4;
73  cmd[(int)'T'].send_sz = 5;
74  cmd[(int)'T'].read_sz = 2;
75 
76  return status;
77 }
78 
80  hdr.size = 3;
81  hdr.data[0] = 1; //convention
82  hdr.data[1] = _kataddr; //complete header
83 }
84 
85 bool CCplSerialCRC::init(CCdlBase* _device, byte _kataddr) {
86  device = _device;
87  defineProtocol(_kataddr);
88  return load_tbl();
89 }
90 
91 
92 void CCplSerialCRC::comm(const byte* pack, byte* buf, byte* size) {
93  // This method enssamble the packet with the header, data, and CRC.
94  // Sends it and receives the answer.
95  memset(send_buf,0,256); //override old values
96  hdr.data[hdr.size-1] = cmd[pack[0]].send_sz; //complete header
97  memcpy(send_buf, hdr.data, hdr.size);
98  memcpy(send_buf+hdr.size,pack,hdr.data[hdr.size-1]);
99 
100  short crc = CRC_CHECKSUM((uint8*)pack,hdr.data[hdr.size-1]);
101  byte bufsz = hdr.size + hdr.data[hdr.size-1];
102  send_buf[bufsz++] = (byte)(crc >> 8); //hi-byte
103  send_buf[bufsz++] = (byte)(crc & 0xFF); //lo-byte
104 
105  memset(read_buf,0,256); //read through device
106  byte read_sz = cmd[pack[0]].read_sz + 2;
107 
108  short tries_recv = 0;
109  while(true) {
110 
111 
112  try {
113  // uncomment to get debug output:
114  // if (pack[0] != 'N') {
115  // // N = Get all axis's positions at once
116  // printf("KNI >>> %c", pack[0]);
117  // for (size_t i = 1; i < cmd[pack[0]].send_sz; i++)
118  // printf(" %d", pack[i]);
119  // printf("\n");
120  // }
121  send(send_buf, bufsz, NUMBER_OF_RETRIES_SEND); // pass exceptions from this since already retried
122  }
123  catch(...){
124  throw;
125  }
126  //For comm DEBUG only: std::cout << "send OK, try to receive...\n";
127  try {
128  recv(read_buf,read_sz,size);
129 
130  // uncomment to get debug output:
131  // if (read_buf[0] != 'n')
132  // {
133  // printf("KNI <<< %c", read_buf[0]);
134  // for (size_t i = 1; i < *size; i++)
135  // printf(" %d", read_buf[i]);
136  // printf("\n");
137  // }
138  memcpy(buf,read_buf,*size); // copy read_buf to _buf
139  } catch ( ReadNotCompleteException & ) {
140  if(tries_recv < NUMBER_OF_RETRIES_RECV) {
141  ++tries_recv;
142  continue;
143  }
144  throw;
145 
146  } catch ( WrongCRCException & ) {
147  if(tries_recv < NUMBER_OF_RETRIES_RECV) {
148  ++tries_recv;
149  continue;
150  } else {
151  throw;
152  }
153  } catch ( FirmwareException & ) {
154  throw;
155  } catch ( Exception & ) {
156  throw;
157  // FIXME why? --MRE
158 // if(errorFlag == true){
159 // }
160  } catch(...){
161  throw;
162  }
163  break; // write succeeded
164 
165  }
166 }
167 
168 void CCplSerialCRC::getMasterFirmware(short* fw, short* rev) {
169  *fw = mMasterVersion;
170  *rev = mMasterRevision;
171 }
172 
173 void CCplSerialCRC::send(byte* send_buf, byte bufsz, short retries) {
174  short r = 0;
175  while(true) {
176  try {
177  device->send(send_buf, bufsz); //send to device
178  } catch ( WriteNotCompleteException & ) {
179  if(r < retries) {
180  ++r;
181  continue;
182  } else {
183  throw;
184  }
185  } catch ( Exception & ) {
186  throw; // throw other exceptions immediately
187  }
188  break;
189  }
190 }
191 
193 
194  // Receives the packet and checks the CRC.
195  *size = device->recv(read_buf, read_sz); //receives from device
196 
197  bool getErrorMessage = false;
198  //check for error from Katana:
199  if(read_buf[0] == KATANA_ERROR_FLAG){
200  std::cout << "Error flag received:\n";
201  assert(*size == 3);
202  getErrorMessage = true;
203  read_sz = *size; // FIXME: should not return the now invalid buffer?
204  } else {
205  if (*size != read_sz) {
206  throw ReadNotCompleteException("?"); // FIXME: should get _ipAddr for nicer error message
207  }
208  }
209 
210  *size -= 2;
211  byte bhi = read_buf[read_sz-2];
212  byte blo = read_buf[read_sz-1];
213 
214  short crc = CRC_CHECKSUM((uint8*)read_buf,*size);
215  byte hi = (byte)(crc >> 8);
216  byte lo = (byte)(crc & 0xFF);
217 
218  if ((hi != bhi) || (lo != blo)) {
219  std::cout << "warning: crc error, throwing exception" << std::endl;
220  throw WrongCRCException();
221  }
222 
223  if (getErrorMessage) {
224  byte buf[57];
225  buf[0] = 0; // ignored
226  buf[1] = 0; // ignored
227  buf[2] = 0; // ignored
228  buf[3] = KATANA_ERROR_FLAG+1;
229  try {
230  send(buf, 4, 1);
231  byte size_errormsg = 57;
232  recv(buf, 57, &size_errormsg);
233  } catch (...) {
234  std::cout << "Error while requesting error message!\n";
235  }
236 
237  if (buf[0] != KATANA_ERROR_FLAG+1) {
238  std::cout << "bad response to error request\n";
239  }
240  byte lastCommand = buf[1];
241  byte errorCode = buf[2];
242  byte device = buf[3];
243  std::string errorString((char*)buf+4);
244  if (device != 0) {
245  errorString += " (axis ";
246  errorString += '0' + device;
247  errorString += ")";
248  }
249  //std::cout << "\"" << errorString << "\"\n";
250  throw FirmwareException(errorString, static_cast<signed char>(errorCode), device, lastCommand);
251  }
252 }
virtual int recv(void *_buf, int _sz)=0
Pure function to receive data.
unsigned char byte
type specification (8 bit)
Definition: cdlBase.h:29
byte send_sz
send size of the packet
Definition: cplSerial.h:83
virtual void getMasterFirmware(short *fw, short *rev)
Get the master firmware of the robot we are communicating with.
Definition: cplSerial.cpp:168
#define NUMBER_OF_RETRIES_SEND
Definition: cplSerial.h:31
virtual int send(const void *_buf, int _sz)=0
Pure function to send data.
TPacket cmd[256]
command table
Definition: cplSerial.h:96
virtual bool init(CCdlBase *_device, byte _kataddr=24)
Initializing function.
Definition: cplSerial.cpp:85
Exception reported by the firmware.
Definition: cplSerial.h:52
uint16 CRC_CHECKSUM(uint8 *data, uint8 size_of_BYTE)
Definition: CRC.cpp:98
byte data[256]
data part: 16x zero, 1x one, 1x katadr
Definition: cplSerial.h:77
#define uint8
unsigned 8 bit
Definition: CRC.h:27
CRC check for the answer package failed.
Definition: cplSerial.h:44
byte size
header size
Definition: cplSerial.h:76
virtual void defineProtocol(byte _kataddr)
Defines the protocol&#39;s attributes.
Definition: cplSerial.cpp:79
byte send_buf[256]
sending buffer
Definition: cplSerial.h:98
const int KATANA_ERROR_FLAG
defines the error flag number
Definition: cplSerial.h:36
virtual void recv(byte *read_buf, byte read_sz, byte *size)
Definition: cplSerial.cpp:192
virtual void comm(const byte *pack, byte *buf, byte *size)
Communication function.
Definition: cplSerial.cpp:92
#define NUMBER_OF_RETRIES_RECV
Definition: cplSerial.h:32
THeader hdr
header
Definition: cplSerial.h:95
byte read_sz
read size of the packet
Definition: cplSerial.h:84
short mMasterVersion
master version of robot we are communicating with
Definition: cplBase.h:51
virtual bool load_tbl()
Loads the command table from the robot&#39;s firmware.
Definition: cplSerial.cpp:33
virtual void send(byte *send_buf, byte write_sz, short retries=3)
Definition: cplSerial.cpp:173
CCdlBase * device
communication device
Definition: cplBase.h:50
byte read_buf[256]
receive buffer
Definition: cplSerial.h:99
byte size
Definition: kni_wrapper.cpp:35
Abstract base class for devices.
Definition: cdlBase.h:47
short mMasterRevision
master firmware revision
Definition: cplBase.h:52


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16