kmlBase.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/kmlBase.h"
22 
23 #include "common/Timer.h"
24 
25 #include <vector>
28  const TKatGNL _gnl,
29  const TKatMOT _mot,
30  const TKatSCT _sct,
31  const TKatEFF _eff,
32  CCplBase* _protocol) {
33 
34  //init vars
35  gnl = _gnl;
36  mot = _mot;
37  sct = _sct;
38  eff = _eff;
39 
40  protocol = _protocol;
41 
42  //init motors
43  mot.arr = new CMotBase[mot.cnt];
44  for (int m=0; m<mot.cnt; m++) {
45  if (!mot.arr[m].init(this,mot.desc[m],protocol)) {
46  delete[] mot.arr;
47  return false;
48  }
49  }
50 
51  //init sensor contollers
52  sct.arr = new CSctBase[sct.cnt];
53  for (int s=0; s<sct.cnt; s++) {
54  if (!sct.arr[s].init(this,sct.desc[s],protocol)) {
55  delete[] sct.arr;
56  return false;
57  }
58  }
59  return true;
60 }
63  byte p[32]; //packet
64  byte buf[256]; //readbuf
65  byte sz = 0; //readbuf size
66  p[0] = 'B';
67 
68  protocol->comm(p,buf,&sz);
69 
70  mfw.ver = buf[1];
71  mfw.rev = buf[2];
72 
73 }
76  byte p[32]; //packet
77  byte buf[256]; //readbuf
78  byte sz = 0; //readbuf size
79  p[0] = 'Y';
80 
81  protocol->comm(p,buf,&sz);
82 
83  memcpy(ids.strID,buf+1,sz-1);
84  ids.strID[sz-3] = 0;
85 
86 }
89  byte p[32]; //packet
90  byte buf[256]; //readbuf
91  byte sz = 0; //readbuf size
92  p[0] = 'X';
93 
94  protocol->comm(p,buf,&sz);
95 
96  memcpy(ctb.cmdtbl,buf+1,sz-1);
97  ctb.cmdtbl[sz-1] = 0;
98 
99 }
101 //'G'et every 'M'otor's 'S'tatus flag
103  int i;
104  byte p[32]; //packet
105  byte buf[256]; //readbuf
106  byte sz = 0; //readbuf size
107  p[0] = 'N';
108  p[1] = 1;
109  p[2] = 0;
110 
111  protocol->comm(p,buf,&sz);
112 
113  for (i=0; i<mot.cnt; i++) {
114  mot.arr[i].pvp.msf = (TMotStsFlg)buf[i+1];
115  }
116 }
118 void CKatBase::waitFor(TMotStsFlg status, int waitTimeout, bool gripper) {
119  KNI::Timer t(waitTimeout);
120  t.Start();
121  int nOfMot = GetMOT()->cnt;
122  if (gripper)
123  nOfMot--;
124  //win32 compiler compatibility
125  bool reached[10];
126  for (int i = 0; i < nOfMot; ++i) {
127  reached[i] = false;
128  }
129  bool reachedall;
130  while (true) {
131  if (t.Elapsed())
132  throw MotorTimeoutException();
133  recvGMS();
134  reachedall = true;
135  for (int i = 0; i < nOfMot; ++i) {
136  if (mot.arr[i].pvp.msf == 40)
137  throw MotorCrashException();
138  if ((reached[i] == false) && (mot.arr[i].pvp.msf == status)) {
139  reached[i] = true;
140  }
141  if (reached[i] == false)
142  reachedall = false;
143  }
144  if (reachedall)
145  return;
146  KNI::sleep(1000);
147  }
148 }
151  byte p[32]; //packet
152  byte buf[256]; //readbuf
153  byte sz = 0; //readbuf size
154  p[0] = 'Z';
155 
156  protocol->comm(p,buf,&sz);
157 
158  ech.echo = buf[0];
159  if (ech.echo != 'z') {
160  throw ParameterReadingException("ECH");
161  }
162 }
164 void CKatBase::getMasterFirmware(short* fw, short* rev) {
165  *fw = mMasterVersion;
166  *rev = mMasterRevision;
167 }
170  // adjust second byte of packet according to katana model
171  short version, revision;
172  int motor = 1; // master ON with KatHD300
173  getMasterFirmware(&version, &revision);
174  if (checkKatanaType(400)) {
175  motor = 0; // switch all motors with KatHD400
176  }
177  byte p[32]; //packet
178  byte buf[256]; //readbuf
179  byte sz = 0; //readbuf size
180  p[0] = 'A';
181  p[1] = motor;
182  p[2] = 1;
183  protocol->comm(p,buf,&sz);
184 }
187  byte p[32]; //packet
188  byte buf[256]; //readbuf
189  byte sz = 0; //readbuf size
190  p[0] = 'A';
191  p[1] = 0;
192  p[2] = 0;
193  protocol->comm(p,buf,&sz);
194 }
196 //deprecated, use speed & position
197 void CKatBase::setCrashLimit(long idx, int limit) {
198  byte p[32]; //packet
199  byte buf[256]; //readbuf
200  byte sz = 0; //readbuf size
201  p[0] = 'S';
202  p[1] = 5; // subcommand 5 "Set Crashlimit"
203  p[2] = (char)(limit >> 8);
204  p[3] = (char)(limit);
205  p[4] = 0;
206  protocol->comm(p,buf,&sz);
207 }
209 void CKatBase::setPositionCollisionLimit(long idx, int limit){
210  mot.arr[idx].setPositionCollisionLimit(limit);
211 }
213 void CKatBase::setSpeedCollisionLimit(long idx, int limit){
214  mot.arr[idx].setSpeedCollisionLimit(limit);
215 }
218  for (int i=0; i < mot.cnt; ++i) {
219  mot.arr[i].resetBlocked();
220  }
221 }
223 void CKatBase::startSplineMovement(int exactflag, int moreflag) {
224  // Start Linear movement
225  std::vector<byte> sendBuf(3), recvBuf(2, 0);
226  byte readBytes;
227  sendBuf[0] ='G'+128 ;
228  sendBuf[1] = (byte) moreflag;
229  sendBuf[2] = (byte) exactflag;
230  protocol->comm(&sendBuf.front(), &recvBuf.front(), &readBytes);
231 }
233 void CKatBase::setAndStartPolyMovement(std::vector<short> polynomial, int exactflag, int moreflag) {
234  // set and start poly movement on all motors
235  std::vector<byte> sendBuf(75), recvBuf(3, 0);
236  byte readBytes;
237  sendBuf[0] ='H';
238  for (int i = 0; i < (int)polynomial.size(); ++i) {
239  sendBuf[2*i+1] = static_cast<byte>(polynomial[i] >> 8);
240  sendBuf[2*i+2] = static_cast<byte>(polynomial[i]);
241  }
242  sendBuf[73] = (byte) moreflag;
243  sendBuf[74] = (byte) exactflag;
244  protocol->comm(&sendBuf.front(), &recvBuf.front(), &readBytes);
245 }
248  byte p[32]; //packet
249  byte buf[256]; //readbuf
250  byte sz = 0; //readbuf size
251  p[0] = 'N';
252  p[1] = 3;
253  p[2] = 0;
254  protocol->comm(p,buf,&sz);
255  for (int i=0; i<mot.cnt; i++) {
256  mot.arr[i].pvp.pos = (((short)buf[2*i+1]) <<8) | buf[2*i+2];
257  }
258 }
261  if (protocol != NULL) {
262  recvMFW();
263  if((type == 400) || (type == 450)){
265  return -1;
266  }
267  }
268  else if(type == 300){
270  return -1;
271  }
272  }
273  }
274 
275  return 1;
276 }
279  byte p[32];
280  byte buf[256];
281  byte sz =0;
282  p[0] = 'T';
283  p[1] = 'r';
284  p[2] = 0;
285  p[3] = 0;
286  p[4] = 0;
287  protocol->comm(p,buf,&sz);
288  return buf[1];
289 }
292  byte p[32];
293  byte buf[256];
294  byte sz =0;
295  p[0] = 'C';
296  p[1] = 0;
297  p[2] = 32;
298  p[3] = 0;
299  p[4] = 0;
300  protocol->comm(p,buf,&sz);
301  return 1;
302 }
unsigned char byte
type specification (8 bit)
Definition: cdlBase.h:29
TKatMOT mot
motors
Definition: kmlBase.h:142
virtual bool init(const TKatGNL _gnl, const TKatMOT _mot, const TKatSCT _sct, const TKatEFF _eff, CCplBase *_protocol)
Definition: kmlBase.cpp:27
void enableCrashLimits()
crash limits enable
Definition: kmlBase.cpp:169
TMotPVP pvp
reading motor parameters
Definition: kmlMotBase.h:231
TKatMFW mfw
master&#39;s firmware version/revision
Definition: kmlBase.h:136
byte cmdtbl[256]
command table
Definition: kmlBase.h:88
Abstract base class for protocol definiton.
Definition: cplBase.h:47
[SCT] every sens ctrl&#39;s attributes
Definition: kmlSctBase.h:41
bool Elapsed() const
Definition: Timer.cpp:69
TKatEFF eff
end effector
Definition: kmlBase.h:144
void setAndStartPolyMovement(std::vector< short > polynomial, int exactflag, int moreflag)
Definition: kmlBase.cpp:233
void setSpeedCollisionLimit(long idx, int limit)
set collision speed limits
Definition: kmlBase.cpp:213
Sensor Controller class.
Definition: kmlSctBase.h:72
TKatSCT sct
sensor controllers
Definition: kmlBase.h:143
void startSplineMovement(int exactflag, int moreflag=1)
Definition: kmlBase.cpp:223
TSctDesc * desc
description[]
Definition: kmlSctBase.h:44
short mMasterVersion
master version of robot we are communicating with
Definition: kmlBase.h:147
TKatECH ech
echo
Definition: kmlBase.h:140
CMotBase * arr
array of motors
Definition: kmlMotBase.h:42
TMotStsFlg
status flags
Definition: kmlMotBase.h:58
int flushMoveBuffers()
flush move buffers on all motors:
Definition: kmlBase.cpp:291
void disableCrashLimits()
crash limits disable
Definition: kmlBase.cpp:186
void recvMFW()
receive data
Definition: kmlBase.cpp:62
byte rev
revision
Definition: kmlBase.h:76
int readDigitalIO()
get digital I/O data from Katana400:
Definition: kmlBase.cpp:278
void waitFor(TMotStsFlg status, int waitTimeout, bool gripper)
wait for motor on all motors
Definition: kmlBase.cpp:118
const TKatMOT * GetMOT()
Get a pointer to the desired structure.
Definition: kmlBase.h:165
byte echo
echo answer
Definition: kmlBase.h:101
short mMasterRevision
master firmware revision
Definition: kmlBase.h:148
virtual void comm(const byte *pack, byte *buf, byte *size)=0
Base communication function.
void recvMPS()
read all motor positions simultaneously
Definition: kmlBase.cpp:247
Inverse Kinematics structure of the endeffektor.
Definition: kmlBase.h:113
[MOT] every motor&#39;s attributes
Definition: kmlMotBase.h:40
void setPositionCollisionLimit(int limit)
Set the collision limit.
Definition: kmlMotBase.cpp:259
void sleep(long time)
Definition: Timer.cpp:86
void recvGMS()
receive data
Definition: kmlBase.cpp:102
CSctBase * arr
array of sens ctrl&#39;s
Definition: kmlSctBase.h:43
TMotDesc * desc
description[]
Definition: kmlMotBase.h:43
void recvECH()
receive data
Definition: kmlBase.cpp:150
void Start()
Definition: Timer.cpp:51
short pos
position
Definition: kmlMotBase.h:164
bool init(CKatBase *_own, const TMotDesc _motDesc, CCplBase *protocol)
Definition: kmlMotBase.cpp:20
void getMasterFirmware(short *fw, short *rev)
Get the master firmware of the robot we are communicating with.
Definition: kmlBase.cpp:164
[GNL] general robot attributes
Definition: kmlBase.h:67
void resetBlocked()
unblock the motor.
Definition: kmlMotBase.cpp:35
short cnt
count of motors
Definition: kmlMotBase.h:41
TMotStsFlg msf
motor status flag
Definition: kmlMotBase.h:163
#define K400_OLD_PROTOCOL_THRESHOLD
The old protocol is only supported up to K400 version 0.x.x.
Definition: kmlBase.h:42
void setCrashLimit(long idx, int limit)
set collision limits
Definition: kmlBase.cpp:197
byte strID[256]
id string
Definition: kmlBase.h:82
Motor class.
Definition: kmlMotBase.h:220
int checkKatanaType(int type)
checks for a K300 or K400
Definition: kmlBase.cpp:260
TKatCTB ctb
cmd table
Definition: kmlBase.h:138
CCplBase * protocol
protocol interface
Definition: kmlBase.h:146
TKatGNL gnl
katana general
Definition: kmlBase.h:135
void setPositionCollisionLimit(long idx, int limit)
set collision position limits
Definition: kmlBase.cpp:209
TKatIDS ids
ID string.
Definition: kmlBase.h:137
void unBlock()
unblock robot after a crash
Definition: kmlBase.cpp:217
bool init(CKatBase *_own, const TSctDesc _sctDesc, CCplBase *protocol)
Definition: kmlSctBase.cpp:16
void setSpeedCollisionLimit(int limit)
Set the collision limit.
Definition: kmlMotBase.cpp:243
void recvCTB()
receive data
Definition: kmlBase.cpp:88
void recvIDS()
receive data
Definition: kmlBase.cpp:75
short cnt
count of sens ctrl&#39;s
Definition: kmlSctBase.h:42
byte ver
version
Definition: kmlBase.h:75


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