kmlBase.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/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 }


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