$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 00022 /******************************************************************************************************************/ 00023 #ifndef _KMLEXT_H_ 00024 #define _KMLEXT_H_ 00025 /******************************************************************************************************************/ 00026 #include "common/dllexport.h" 00027 #include "common/exception.h" 00028 00029 #include "KNI/kmlBase.h" 00030 #include "KNI/kmlMotBase.h" 00031 00032 #include <vector> 00033 00034 00035 /******************************************************************************************************************/ 00036 00041 00045 class ConfigFileOpenException : public Exception { 00046 public: 00047 ConfigFileOpenException(const std::string & port) throw (): 00048 Exception("Cannot open configuration file '" + port + "'", -40) {} 00049 }; 00050 00054 00055 namespace KNI { 00056 class kmlFactory; 00057 } 00058 00064 class DLLDIR CKatana { 00065 protected: 00066 //-------------------------------------// 00067 CKatBase* base; 00068 00069 bool _gripperIsPresent; 00070 int _gripperOpenEncoders; 00071 int _gripperCloseEncoders; 00073 int mKatanaType; 00075 int mKinematics; 00076 00079 void setTolerance(long idx, int enc_tolerance); 00080 00081 public: 00082 //-------------------------------------// 00083 CKatBase* GetBase() { return base; } 00084 00088 CKatana() { base = new CKatBase; } 00091 ~CKatana() { delete base; } 00092 //------------------------------------------------------------------------------// 00095 void create(const char* configurationFile, CCplBase* protocol); 00096 void create(KNI::kmlFactory* infos, CCplBase* protocol); 00097 00100 void create(TKatGNL& gnl, 00101 TKatMOT& mot, 00102 TKatSCT& sct, 00103 TKatEFF& eff, 00104 CCplBase* protocol 00105 ); 00106 //------------------------------------------------------------------------------// 00107 00108 00109 /* \brief calibrates the robot 00110 */ 00111 void calibrate(); 00112 00113 void calibrate( long idx, 00114 TMotCLB clb, 00115 TMotSCP scp, 00116 TMotDYL dyl 00117 ); 00118 00119 //------------------------------------------------------------------------------// 00120 00121 void searchMechStop(long idx, 00122 TSearchDir dir, 00123 TMotSCP scp, 00124 TMotDYL dyl 00125 ); 00126 00127 00128 //------------------------------------------------------------------------------// 00131 void inc(long idx, int dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS); 00134 void dec(long idx, int dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS); 00138 void mov(long idx, int tar, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS); 00139 00140 //------------------------------------------------------------------------------// 00143 void incDegrees(long idx, double dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS); 00146 void decDegrees(long idx, double dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS); 00150 void movDegrees(long idx, double tar, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS); 00151 00152 //------------------------------------------------------------------------------// 00153 // public just for dubbuging purposes 00157 bool checkENLD(long idx, double degrees); 00158 00159 //------------------------------------------------------------------------------// 00160 00164 void setGripperParameters(bool isPresent, int openEncoders, int closeEncoders); 00166 void getGripperParameters(bool &isPresent, int &openEncoders, int &closeEncoders); 00167 //------------------------------------------------------------------------------// 00168 00171 void enableCrashLimits(); 00174 void disableCrashLimits(); 00177 void unBlock(); 00180 void flushMoveBuffers(); 00183 void setCrashLimit(long idx, int limit); 00186 void setPositionCollisionLimit(long idx, int limit); 00189 void setSpeedCollisionLimit(long idx, int limit); 00193 void setForceLimit(int axis, int limit); 00194 00195 //------------------------------------------------------------------------------// 00196 00197 short getNumberOfMotors() const; 00198 int getMotorEncoders(short number, bool refreshEncoders = true) const; 00199 00203 std::vector<int>::iterator getRobotEncoders(std::vector<int>::iterator start, std::vector<int>::const_iterator end, bool refreshEncoders = true) const; 00204 00208 std::vector<int> getRobotEncoders(bool refreshEncoders = true) const; 00209 00210 short getMotorVelocityLimit( short number ) const; 00211 short getMotorAccelerationLimit( short number ) const; 00212 00213 void setMotorVelocityLimit( short number, short velocity ); 00214 void setMotorAccelerationLimit( short number, short acceleration ); 00217 short getForce(int axis); 00221 int getCurrentControllerType(int axis); 00222 00223 void setRobotVelocityLimit( short velocity ); 00226 void setRobotAccelerationLimit( short acceleration ); 00227 00228 void moveMotorByEnc( short number, int encoders, bool waitUntilReached = false, int waitTimeout = 0); 00229 void moveMotorBy ( short number, double radianAngle, bool waitUntilReached = false, int waitTimeout = 0); 00230 00231 void moveMotorToEnc( short number, int encoders, bool waitUntilReached = false, int encTolerance = 100, int waitTimeout = 0); 00232 void moveMotorTo ( short number, double radianAngle, bool waitUntilReached = false, int waitTimeout = 0); 00233 00234 void waitForMotor( short number, int encoders, int encTolerance = 100, short mode = 0, int waitTimeout = 5000); 00235 void waitFor(TMotStsFlg status, int waitTimeout); 00236 00240 void moveRobotToEnc(std::vector<int>::const_iterator start, std::vector<int>::const_iterator end, bool waitUntilReached = false, int encTolerance = 100, int waitTimeout = 0); 00244 void moveRobotToEnc(std::vector<int> encoders, bool waitUntilReached = false, int encTolerance = 100, int waitTimeout = 0); 00246 void moveRobotToEnc4D(std::vector<int> target, int velocity=180, int acceleration = 1, int encTolerance = 100); 00247 00248 void openGripper (bool waitUntilReached = false, int waitTimeout = 100); 00249 void closeGripper(bool waitUntilReached = false, int waitTimeout = 100); 00250 00251 void freezeRobot(); 00252 void freezeMotor(short number); 00253 void switchRobotOn(); 00254 void switchRobotOff(); 00255 void switchMotorOn(short number); 00256 void switchMotorOff(short number); 00257 00261 void startSplineMovement(bool exactflag, int moreflag = 1); 00262 00266 void sendSplineToMotor(short number, short targetPosition, short duration, short p1, short p2, short p3, short p4); 00267 00272 void setAndStartPolyMovement(std::vector<short> polynomial, bool exactflag, int moreflag); 00273 00275 int readDigitalIO(); 00276 }; 00277 00278 /******************************************************************************************************************/ 00279 #endif //_KMLEXT_H_ 00280 /******************************************************************************************************************/