$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/kmlBase.h" 00022 00023 #include "common/Timer.h" 00024 00025 #include <vector> 00027 bool CKatBase::init( 00028 const TKatGNL _gnl, 00029 const TKatMOT _mot, 00030 const TKatSCT _sct, 00031 const TKatEFF _eff, 00032 CCplBase* _protocol) { 00033 00034 //init vars 00035 gnl = _gnl; 00036 mot = _mot; 00037 sct = _sct; 00038 eff = _eff; 00039 00040 protocol = _protocol; 00041 00042 //init motors 00043 mot.arr = new CMotBase[mot.cnt]; 00044 for (int m=0; m<mot.cnt; m++) { 00045 if (!mot.arr[m].init(this,mot.desc[m],protocol)) { 00046 delete[] mot.arr; 00047 return false; 00048 } 00049 } 00050 00051 //init sensor contollers 00052 sct.arr = new CSctBase[sct.cnt]; 00053 for (int s=0; s<sct.cnt; s++) { 00054 if (!sct.arr[s].init(this,sct.desc[s],protocol)) { 00055 delete[] sct.arr; 00056 return false; 00057 } 00058 } 00059 return true; 00060 } 00062 void CKatBase::recvMFW() { 00063 byte p[32]; //packet 00064 byte buf[256]; //readbuf 00065 byte sz = 0; //readbuf size 00066 p[0] = 'B'; 00067 00068 protocol->comm(p,buf,&sz); 00069 00070 mfw.ver = buf[1]; 00071 mfw.rev = buf[2]; 00072 00073 } 00075 void CKatBase::recvIDS() { 00076 byte p[32]; //packet 00077 byte buf[256]; //readbuf 00078 byte sz = 0; //readbuf size 00079 p[0] = 'Y'; 00080 00081 protocol->comm(p,buf,&sz); 00082 00083 memcpy(ids.strID,buf+1,sz-1); 00084 ids.strID[sz-3] = 0; 00085 00086 } 00088 void CKatBase::recvCTB() { 00089 byte p[32]; //packet 00090 byte buf[256]; //readbuf 00091 byte sz = 0; //readbuf size 00092 p[0] = 'X'; 00093 00094 protocol->comm(p,buf,&sz); 00095 00096 memcpy(ctb.cmdtbl,buf+1,sz-1); 00097 ctb.cmdtbl[sz-1] = 0; 00098 00099 } 00101 //'G'et every 'M'otor's 'S'tatus flag 00102 void CKatBase::recvGMS() { 00103 int i; 00104 byte p[32]; //packet 00105 byte buf[256]; //readbuf 00106 byte sz = 0; //readbuf size 00107 p[0] = 'N'; 00108 p[1] = 1; 00109 p[2] = 0; 00110 00111 protocol->comm(p,buf,&sz); 00112 00113 for (i=0; i<mot.cnt; i++) { 00114 mot.arr[i].pvp.msf = (TMotStsFlg)buf[i+1]; 00115 } 00116 } 00118 void CKatBase::waitFor(TMotStsFlg status, int waitTimeout, bool gripper) { 00119 KNI::Timer t(waitTimeout); 00120 t.Start(); 00121 int nOfMot = GetMOT()->cnt; 00122 if (gripper) 00123 nOfMot--; 00124 //win32 compiler compatibility 00125 bool reached[10]; 00126 for (int i = 0; i < nOfMot; ++i) { 00127 reached[i] = false; 00128 } 00129 bool reachedall; 00130 while (true) { 00131 if (t.Elapsed()) 00132 throw MotorTimeoutException(); 00133 recvGMS(); 00134 reachedall = true; 00135 for (int i = 0; i < nOfMot; ++i) { 00136 if (mot.arr[i].pvp.msf == 40) 00137 throw MotorCrashException(); 00138 if ((reached[i] == false) && (mot.arr[i].pvp.msf == status)) { 00139 reached[i] = true; 00140 } 00141 if (reached[i] == false) 00142 reachedall = false; 00143 } 00144 if (reachedall) 00145 return; 00146 KNI::sleep(1000); 00147 } 00148 } 00150 void CKatBase::recvECH() { 00151 byte p[32]; //packet 00152 byte buf[256]; //readbuf 00153 byte sz = 0; //readbuf size 00154 p[0] = 'Z'; 00155 00156 protocol->comm(p,buf,&sz); 00157 00158 ech.echo = buf[0]; 00159 if (ech.echo != 'z') { 00160 throw ParameterReadingException("ECH"); 00161 } 00162 } 00164 void CKatBase::getMasterFirmware(short* fw, short* rev) { 00165 *fw = mMasterVersion; 00166 *rev = mMasterRevision; 00167 } 00169 void CKatBase::enableCrashLimits() { 00170 // adjust second byte of packet according to katana model 00171 short version, revision; 00172 int motor = 1; // master ON with KatHD300 00173 getMasterFirmware(&version, &revision); 00174 if (checkKatanaType(400)) { 00175 motor = 0; // switch all motors with KatHD400 00176 } 00177 byte p[32]; //packet 00178 byte buf[256]; //readbuf 00179 byte sz = 0; //readbuf size 00180 p[0] = 'A'; 00181 p[1] = motor; 00182 p[2] = 1; 00183 protocol->comm(p,buf,&sz); 00184 } 00186 void CKatBase::disableCrashLimits() { 00187 byte p[32]; //packet 00188 byte buf[256]; //readbuf 00189 byte sz = 0; //readbuf size 00190 p[0] = 'A'; 00191 p[1] = 0; 00192 p[2] = 0; 00193 protocol->comm(p,buf,&sz); 00194 } 00196 //deprecated, use speed & position 00197 void CKatBase::setCrashLimit(long idx, int limit) { 00198 byte p[32]; //packet 00199 byte buf[256]; //readbuf 00200 byte sz = 0; //readbuf size 00201 p[0] = 'S'; 00202 p[1] = 5; // subcommand 5 "Set Crashlimit" 00203 p[2] = (char)(limit >> 8); 00204 p[3] = (char)(limit); 00205 p[4] = 0; 00206 protocol->comm(p,buf,&sz); 00207 } 00209 void CKatBase::setPositionCollisionLimit(long idx, int limit){ 00210 mot.arr[idx].setPositionCollisionLimit(limit); 00211 } 00213 void CKatBase::setSpeedCollisionLimit(long idx, int limit){ 00214 mot.arr[idx].setSpeedCollisionLimit(limit); 00215 } 00217 void CKatBase::unBlock() { 00218 for (int i=0; i < mot.cnt; ++i) { 00219 mot.arr[i].resetBlocked(); 00220 } 00221 } 00223 void CKatBase::startSplineMovement(int exactflag, int moreflag) { 00224 // Start Linear movement 00225 std::vector<byte> sendBuf(3), recvBuf(2, 0); 00226 byte readBytes; 00227 sendBuf[0] ='G'+128 ; 00228 sendBuf[1] = (byte) moreflag; 00229 sendBuf[2] = (byte) exactflag; 00230 protocol->comm(&sendBuf.front(), &recvBuf.front(), &readBytes); 00231 } 00233 void CKatBase::setAndStartPolyMovement(std::vector<short> polynomial, int exactflag, int moreflag) { 00234 // set and start poly movement on all motors 00235 std::vector<byte> sendBuf(75), recvBuf(3, 0); 00236 byte readBytes; 00237 sendBuf[0] ='H'; 00238 for (int i = 0; i < (int)polynomial.size(); ++i) { 00239 sendBuf[2*i+1] = static_cast<byte>(polynomial[i] >> 8); 00240 sendBuf[2*i+2] = static_cast<byte>(polynomial[i]); 00241 } 00242 sendBuf[73] = (byte) moreflag; 00243 sendBuf[74] = (byte) exactflag; 00244 protocol->comm(&sendBuf.front(), &recvBuf.front(), &readBytes); 00245 } 00247 void CKatBase::recvMPS() { 00248 byte p[32]; //packet 00249 byte buf[256]; //readbuf 00250 byte sz = 0; //readbuf size 00251 p[0] = 'N'; 00252 p[1] = 3; 00253 p[2] = 0; 00254 protocol->comm(p,buf,&sz); 00255 for (int i=0; i<mot.cnt; i++) { 00256 mot.arr[i].pvp.pos = (((short)buf[2*i+1]) <<8) | buf[2*i+2]; 00257 } 00258 } 00260 int CKatBase::checkKatanaType(int type){ 00261 if (protocol != NULL) { 00262 recvMFW(); 00263 if((type == 400) || (type == 450)){ 00264 if(mfw.ver > K400_OLD_PROTOCOL_THRESHOLD){ 00265 return -1; 00266 } 00267 } 00268 else if(type == 300){ 00269 if(mfw.ver < K400_OLD_PROTOCOL_THRESHOLD){ 00270 return -1; 00271 } 00272 } 00273 } 00274 00275 return 1; 00276 } 00278 int CKatBase::readDigitalIO(){ 00279 byte p[32]; 00280 byte buf[256]; 00281 byte sz =0; 00282 p[0] = 'T'; 00283 p[1] = 'r'; 00284 p[2] = 0; 00285 p[3] = 0; 00286 p[4] = 0; 00287 protocol->comm(p,buf,&sz); 00288 return buf[1]; 00289 } 00291 int CKatBase::flushMoveBuffers(){ 00292 byte p[32]; 00293 byte buf[256]; 00294 byte sz =0; 00295 p[0] = 'C'; 00296 p[1] = 0; 00297 p[2] = 32; 00298 p[3] = 0; 00299 p[4] = 0; 00300 protocol->comm(p,buf,&sz); 00301 return 1; 00302 }