kni_wrapper.cpp
Go to the documentation of this file.
00001 /**************************************************************************
00002  * kni_wrapper.cpp - 
00003  * Implements a wrapper for the kni library so that the C++ library 
00004  * can be accessed by C based environments (MatLab, LabView)
00005  * Copyright (C) Neuronics AG
00006  * Philipp Keller, Tino Perucchi, 2008
00007 **************************************************************************/
00008 #include <iostream>
00009 #include <memory>
00010 #include <math.h>
00011 #include <vector>
00012 #include <string.h>
00013 #include <map>
00014 
00015 #define EXPORT_FCNS
00016 #include "kni_wrapper/kni_wrapper.h"
00017 
00018 //#include <pthread.h>
00019 #include <sstream>
00021 //KNI internal types
00022 static std::auto_ptr<CLMBase> katana;
00023 static std::auto_ptr<CCdlSocket> device;
00024 static std::auto_ptr<CCplSerialCRC> protocol;
00026 //A map containing movement vectors accessible by a string name:
00027 std::map< std::string, std::vector<TMovement> > movements;
00028 //A vector for storing encoders
00029 std::vector<int> encoders;
00030 //The number of motors, initialized in initKatana()
00031 int numberOfMotors;
00032 //variables to store communication data
00033 byte    packet[32];     //comm packet
00034 byte    buffer[256];    //comm readbuf
00035 byte    size = 0;       //comm readbuf size
00037 
00038 
00039 DLLEXPORT int initKatana(char* configFile, char* ipaddress){
00040         try {
00041                 int port = 5566;
00042                 device.reset(new CCdlSocket(ipaddress, port));
00043                 protocol.reset(new CCplSerialCRC());
00044                 protocol->init(device.get());
00045                 katana.reset(new CLMBase());
00046                 katana->create(configFile, protocol.get());
00047                 numberOfMotors = katana->getNumberOfMotors();
00048                 for(int i = 0; i < numberOfMotors; i++){
00049                         encoders.push_back(0);
00050                 }
00051                 //MessageBox(NULL, "Katana successfully initiated!",TEXT(""),MB_OK);
00052         }
00053         catch(Exception &e){
00054                 std::cout << "ERROR: " << e.message() << std::endl;
00055                 #ifdef WIN32
00056                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00057                 #endif
00058                 return ERR_FAILED;
00059         }
00060         return ERR_SUCCESS;
00061 }
00063 DLLEXPORT int calibrate(int axis){
00064         try{
00065                 katana->calibrate();
00066         }
00067         catch(Exception &e){
00068                 std::cout << "ERROR: " << e.message() << std::endl;
00069                 #ifdef WIN32
00070                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00071                 #endif
00072                 return ERR_FAILED;
00073         }
00074         return ERR_SUCCESS;
00075 }
00077 DLLEXPORT int moveMot(int axis, int enc, int speed, int accel){
00078         try{
00079                 //set speed
00080                 katana->setMotorVelocityLimit(axis-1, speed);
00081                 //set acceleration
00082                 katana->setMotorAccelerationLimit(axis-1, accel);
00083                 //move
00084                 katana->moveMotorToEnc(axis-1, enc);
00085         }
00086         catch(...){
00087                 return ERR_FAILED;
00088         }
00089         return ERR_SUCCESS;
00090 }
00092 DLLEXPORT int waitForMot(int axis, int targetpos, int tolerance){
00093         try{
00094                 katana->waitForMotor( (short)axis-1, targetpos, tolerance, 0, TM_ENDLESS);
00095         }
00096         catch(Exception &e){
00097                 std::cout << "ERROR: " << e.message() << std::endl;
00098                 #ifdef WIN32
00099                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00100                 #endif
00101                 return ERR_FAILED;
00102         }
00103         return ERR_SUCCESS;
00104 }
00106 DLLEXPORT int moveMotAndWait(int axis, int targetpos, int tolerance){
00107         try{
00108                 katana->moveMotorToEnc(axis-1, targetpos);
00109                 waitForMot(axis, targetpos, tolerance);
00110         }
00111         catch(Exception &e){
00112                 std::cout << "ERROR: " << e.message() << std::endl;
00113                 #ifdef WIN32
00114                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00115                 #endif
00116                 return ERR_FAILED;
00117         }
00118         return ERR_SUCCESS;
00119 }
00121 DLLEXPORT int motorOn(int axis){
00122         try{
00123                 katana->switchMotorOn(axis-1);
00124         }
00125         catch(Exception &e){
00126                 std::cout << "ERROR: " << e.message() << std::endl;
00127                 #ifdef WIN32
00128                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00129                 #endif
00130                 return ERR_FAILED;
00131         }
00132         return ERR_SUCCESS;
00133 }
00135 DLLEXPORT int motorOff(int axis){
00136         try{
00137                 katana->switchMotorOff(axis-1);
00138         }
00139         catch(Exception &e){
00140                 std::cout << "ERROR: " << e.message() << std::endl;
00141                 #ifdef WIN32
00142                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00143                 #endif
00144                 return ERR_FAILED;
00145         }
00146         return ERR_SUCCESS;
00147 }
00149 DLLEXPORT int allMotorsOff(){
00150         try{
00151                 katana->switchRobotOff();
00152         }
00153         catch(Exception &e){
00154                 std::cout << "ERROR: " << e.message() << std::endl;
00155                 #ifdef WIN32
00156                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00157                 #endif
00158                 return ERR_FAILED;
00159         }
00160         return ERR_SUCCESS;
00161 }
00163 DLLEXPORT int allMotorsOn(){
00164         try{
00165                 katana->switchRobotOn();
00166         }
00167         catch(Exception &e){
00168                 std::cout << "ERROR: " << e.message() << std::endl;
00169                 #ifdef WIN32
00170                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00171                 #endif
00172                 return ERR_FAILED;
00173         }
00174         return ERR_SUCCESS;
00175 }
00177 DLLEXPORT int setGripper(bool hasGripper){
00178         try{
00179                 // cannot extract configured values from KNI atm.
00180                 int openEncoders = 30770;
00181                 int closeEncoders = 12240;
00182                 
00183                 katana->setGripperParameters(hasGripper, openEncoders, closeEncoders);
00184         }
00185         catch(Exception &e){
00186                 std::cout << "ERROR: " << e.message() << std::endl;
00187                 #ifdef WIN32
00188                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00189                 #endif
00190                 return ERR_FAILED;
00191         }
00192         return ERR_SUCCESS;
00193 }
00195 DLLEXPORT int closeGripper(){
00196         try{
00197                 katana->closeGripper();
00198         }
00199         catch(Exception &e){
00200                 std::cout << "ERROR: " << e.message() << std::endl;
00201                 #ifdef WIN32
00202                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00203                 #endif
00204                 return ERR_FAILED;
00205         }
00206         return ERR_SUCCESS;
00207 }
00209 DLLEXPORT int openGripper(){
00210         try{
00211                 katana->openGripper();
00212         }
00213         catch(Exception &e){
00214                 std::cout << "ERROR: " << e.message() << std::endl;
00215                 #ifdef WIN32
00216                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00217                 #endif
00218                 return ERR_FAILED;
00219         }
00220         return ERR_SUCCESS;
00221 }
00223 DLLEXPORT int moveToPosEnc(int enc1, int enc2, int enc3, int enc4, int enc5, int enc6, int velocity, int acceleration, int tolerance, bool _wait){
00224         std::cout << "moving to: " << enc1 << ", " << enc2 << ", " << enc3 << ", " << enc4 << ", " << enc5 << ", " << enc6 << "\n";
00225         try{
00226                 for(int i = 0; i < numberOfMotors; i++){
00227                         //set speed
00228                         katana->setMotorVelocityLimit(i, velocity);
00229                         //set acceleration
00230                         katana->setMotorAccelerationLimit(i, acceleration);
00231                 }
00232                 //move
00233                 std::vector<int> enc;
00234                 enc.push_back(enc1);
00235                 enc.push_back(enc2);
00236                 enc.push_back(enc3);
00237                 enc.push_back(enc4);
00238                 enc.push_back(enc5);
00239                 enc.push_back(enc6);
00240                 katana->moveRobotToEnc(enc.begin(), enc.end(), _wait, tolerance);
00241         }
00242         catch(Exception &e){
00243                 std::cout << "ERROR: " << e.message() << std::endl;
00244                 #ifdef WIN32
00245                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00246                 #endif
00247                 return ERR_FAILED;
00248         }
00249         return ERR_SUCCESS;
00250 }
00252 DLLEXPORT int sendSplineToMotor(int axis, int targetpos, int duration, int p0, int p1, int p2, int p3){
00253         try{
00254                 katana->sendSplineToMotor((unsigned short) (axis-1), (short) targetpos, (short) duration, p0, p1, p2, p3);
00255         }
00256         catch(Exception &e){
00257                 std::cout << "ERROR: " << e.message() << std::endl;
00258                 #ifdef WIN32
00259                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00260                 #endif
00261                 return ERR_FAILED;
00262         }
00263         return ERR_SUCCESS;
00264 }
00266 DLLEXPORT int startSplineMovement(int contd, int exactflag){
00267         try{
00268                 bool exact = (exactflag != 0);
00269                 katana->startSplineMovement(exact, contd);
00270         }
00271         catch(Exception &e){
00272                 std::cout << "ERROR: " << e.message() << std::endl;
00273                 #ifdef WIN32
00274                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00275                 #endif
00276                 return ERR_FAILED;
00277         }
00278         return ERR_SUCCESS;
00279 }
00281 DLLEXPORT int flushMoveBuffers(){
00282         try{
00283                 katana->flushMoveBuffers();
00284         }
00285         catch(Exception &e){
00286                 std::cout << "ERROR: " << e.message() << std::endl;
00287                 #ifdef WIN32
00288                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00289                 #endif
00290                 return ERR_FAILED;
00291         }
00292         return ERR_SUCCESS;
00293 }
00295 DLLEXPORT int getPosition(struct TPos *pos){
00296         try{
00297                 katana->getCoordinates(pos->X, pos->Y, pos->Z, pos->phi, pos->theta, pos->psi);
00298         }
00299         catch(Exception &e){
00300                 std::cout << "ERROR: " << e.message() << std::endl;
00301                 #ifdef WIN32
00302                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00303                 #endif
00304                 return ERR_FAILED;
00305         }
00306         return ERR_SUCCESS;
00307 }
00309 DLLEXPORT int moveToPos(struct TPos *pos, int velocity, int acceleration){
00310         try{
00311                 setMaxAccel(0, acceleration);
00312                 setMaxVelocity(0, velocity);
00313                 katana->moveRobotTo(pos->X, pos->Y, pos->Z, pos->phi, pos->theta, pos->psi);
00314         }
00315         catch(Exception &e){
00316                 std::cout << "ERROR: " << e.message() << std::endl;
00317                 #ifdef WIN32
00318                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00319                 #endif
00320                 return ERR_FAILED;
00321         }
00322         return ERR_SUCCESS;
00323 }
00325 DLLEXPORT int moveToPosLin(struct TPos *targetPos, int velocity, int acceleration){
00326         try{
00327                 setMaxAccel(0, acceleration);
00328                 setMaxVelocity(0, velocity);
00329                 katana->moveRobotLinearTo(targetPos->X, targetPos->Y, targetPos->Z, targetPos->phi, targetPos->theta, targetPos->psi);
00330         }
00331         catch(Exception &e){
00332                 std::cout << "ERROR: " << e.message() << std::endl;
00333                 #ifdef WIN32
00334                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00335                 #endif
00336                 return ERR_FAILED;
00337         }
00338         return ERR_SUCCESS;
00339 }
00341 DLLEXPORT int pushMovementToStack(struct TMovement *movement, char* name){
00342         try{
00343                 // name as string
00344                 std::string name_str(name);
00345                 // get according movement vector, a new entry is created and inserted
00346                 // automatically if it does not exist and store movement in it
00347                 movements[name_str].push_back(*movement);
00348         }
00349         catch(Exception &e){
00350                 std::cout << "ERROR: " << e.message() << std::endl;
00351                 #ifdef WIN32
00352                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00353                 #endif
00354                 return ERR_FAILED;
00355         }
00356         return ERR_SUCCESS;
00357 }
00359 DLLEXPORT int deleteMovementFromStack(char* name, int index){
00360         try{
00361                 int status = ERR_SUCCESS;
00362                 // name as string
00363                 std::string name_str(name);
00364                 // find movement vector
00365                 std::map< std::string, std::vector<TMovement> >::iterator it;
00366                 it = movements.find(name_str);
00367                 if (it == movements.end()) {
00368                         // movement vector does not exist
00369                         status = ERR_FAILED;
00370                 } else {
00371                         // movement vector exists
00372                         std::vector<TMovement> movement_vector = (*it).second;
00373                         if ((int)movement_vector.size() >= index) {
00374                                 // index out of range
00375                                 status = ERR_FAILED;
00376                         } else {
00377                                 // erase movement
00378                                 movement_vector.erase(movement_vector.begin()+index);
00379                                 status = ERR_SUCCESS;
00380                         }
00381                 }
00382                 if (status == ERR_FAILED) {
00383                         return ERR_FAILED;
00384                 }
00385         }
00386         catch(Exception &e){
00387                 std::cout << "ERROR: " << e.message() << std::endl;
00388                 #ifdef WIN32
00389                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00390                 #endif
00391                 return ERR_FAILED;
00392         }
00393         return ERR_SUCCESS;
00394 }
00396 DLLEXPORT int deleteMovementStack(char* name){
00397         try{
00398                 int status = ERR_SUCCESS;
00399                 // name as string
00400                 std::string name_str(name);
00401                 // find movement vector
00402                 std::map< std::string, std::vector<TMovement> >::iterator it;
00403                 it = movements.find(name_str);
00404                 if (it == movements.end()) {
00405                         // movement vector does not exist
00406                         status = ERR_FAILED;
00407                 } else {
00408                         // movement vector exists, erase it
00409                         movements.erase(it);
00410                         status = ERR_SUCCESS;
00411                 }
00412                 if (status == ERR_FAILED) {
00413                         return ERR_FAILED;
00414                 }
00415         }
00416         catch(Exception &e){
00417                 std::cout << "ERROR: " << e.message() << std::endl;
00418                 #ifdef WIN32
00419                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00420                 #endif
00421                 return ERR_FAILED;
00422         }
00423         return ERR_SUCCESS;
00424 }
00426 DLLEXPORT int runThroughMovementStack(char* name, int loops){
00427         try{
00428                 int status = ERR_SUCCESS;
00429                 // name as string
00430                 std::string name_str(name);
00431                 // find movement vector
00432                 std::map< std::string, std::vector<TMovement> >::iterator it;
00433                 it = movements.find(name_str);
00434                 if (it == movements.end()) {
00435                         // movement vector does not exist
00436                         status = ERR_FAILED;
00437                 } else {
00438                         // movement vector exists
00439                         std::vector<TMovement> movement_vector = (*it).second;
00440                         // execute all movements in movement vector
00441                         int size = (int)movement_vector.size();
00442                         if (size == 1) {
00443                                 status = executeMovement(&(movement_vector.at(0)));
00444                         } else {
00445                                 bool first, last;
00446                                 struct TPos *lastpos;
00447                                 for (int j = 0; j < loops; ++j) {
00448                                         last = false;
00449                                         for (int i = 0; i < size; ++i) {
00450                                                 if (i == 0) {
00451                                                         first = true;
00452                                                         lastpos = NULL;
00453                                                 } else {
00454                                                         first = false;
00455                                                         lastpos = &((movement_vector.at(i-1)).pos);
00456                                                 }
00457                                                 if (i == (size - 1))
00458                                                         last = true;
00459                                                 status = executeConnectedMovement(&(movement_vector.at(i)),
00460                                                                 lastpos, first, last);
00461                                                 if (status == ERR_FAILED) {
00462                                                         break;
00463                                                 }
00464                                         }
00465                                 }
00466                         }
00467                 }
00468                 if (status == ERR_FAILED) {
00469                         return ERR_FAILED;
00470                 }
00471         }
00472         catch(Exception &e){
00473                 std::cout << "ERROR: " << e.message() << std::endl;
00474                 #ifdef WIN32
00475                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00476                 #endif
00477                 return ERR_FAILED;
00478         }
00479         return ERR_SUCCESS;
00480 }
00482 DLLEXPORT int executeMovement(struct TMovement *movement){
00483         int status = ERR_SUCCESS;
00484         if (movement->transition == PTP) {
00485                 //move PTP
00486                 status = moveToPos(&(movement->pos), movement->velocity, movement->acceleration);
00487         } else if (movement->transition == LINEAR) {
00488                 //move linear
00489                 status = moveToPosLin(&(movement->pos), movement->velocity, movement->acceleration);
00490         } else {
00491                 status = ERR_FAILED;
00492         }
00493         return status;
00494 }
00496 DLLEXPORT int executeConnectedMovement(struct TMovement *movement, struct TPos *startPos,
00497                 bool first, bool last){
00498         int status = ERR_SUCCESS;
00499         setMaxAccel(0, movement->acceleration);
00500         setMaxVelocity(0, movement->velocity);
00501         bool wait = last; // only wait at the last of the connected movements
00502         if (movement->transition == PTP) {
00503                 //move PTP
00504                 try{
00505                         if (first) {
00506                                 // first of the connected movements, no startpos known
00507                                 katana->moveRobotTo(movement->pos.X, movement->pos.Y, movement->pos.Z,
00508                                                 movement->pos.phi, movement->pos.theta, movement->pos.psi,
00509                                                 wait);
00510                         } else {
00511                                 // startpos known
00512                                 katana->movP2P(startPos->X, startPos->Y, startPos->Z, startPos->phi,
00513                                                 startPos->theta, startPos->psi, movement->pos.X, movement->pos.Y,
00514                                                 movement->pos.Z, movement->pos.phi, movement->pos.theta,
00515                                                 movement->pos.psi, true, movement->velocity, wait);
00516                         }
00517                 }
00518                 catch(...){
00519                         status = ERR_FAILED;
00520                 }
00521         } else if (movement->transition == LINEAR) {
00522                 //move linear
00523                 try{
00524                         if (first) {
00525                                 // first of the connected movements, no startpos known
00526                                 katana->moveRobotLinearTo(movement->pos.X, movement->pos.Y,
00527                                                 movement->pos.Z, movement->pos.phi, movement->pos.theta,
00528                                                 movement->pos.psi, wait);
00529                         } else {
00530                                 // startpos known
00531                                 katana->movLM2P(startPos->X, startPos->Y, startPos->Z, startPos->phi,
00532                                                 startPos->theta, startPos->psi, movement->pos.X, movement->pos.Y,
00533                                                 movement->pos.Z, movement->pos.phi, movement->pos.theta,
00534                                                 movement->pos.psi, true, movement->velocity, wait);
00535                         }
00536                 }
00537                 catch(...){
00538                         status = ERR_FAILED;
00539                 }
00540         } else {
00541                 status = ERR_FAILED;
00542         }
00543         return status;
00544 }
00546 DLLEXPORT int ModBusTCP_writeWord(int address, int value){
00547         try{
00548                 byte packet[32];
00549                 byte size;
00550                 packet[0] = 'M';
00551                 packet[1] = 'W';
00552                 packet[2] = (byte)address;
00553                 packet[3] = (byte)(value >> 8);
00554                 packet[4] = (byte)value;
00555                 protocol->comm(packet, buffer, &size);
00556                 if (!buffer[0] || ((short) size != 4)){
00557                         return ERR_FAILED;
00558                 }
00559         }
00560         catch(Exception &e){
00561                 std::cout << "ERROR: " << e.message() << std::endl;
00562                 #ifdef WIN32
00563                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00564                 #endif
00565                 return ERR_FAILED;
00566         }
00567         return ERR_SUCCESS;
00568 }
00570 int ModBusTCP_readWord(int address, int &value){
00571         try{
00572                 byte packet[32];
00573                 byte size;
00574                 packet[0] = 'M';
00575                 packet[1] = 'R';
00576                 packet[2] = (byte)address;
00577                 protocol->comm(packet, buffer, &size);
00578                 if (!buffer[0] || ((short) size != 4)){
00579                         return ERR_FAILED;
00580                 }
00581                 int val = buffer[2];
00582                 val <<= 8;
00583                 val += buffer[1];
00584                 value = val;
00585         }
00586         catch(Exception &e){
00587                 std::cout << "ERROR: " << e.message() << std::endl;
00588                 #ifdef WIN32
00589                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00590                 #endif
00591                 return ERR_FAILED;
00592         }
00593         return ERR_SUCCESS;
00594 }
00596 DLLEXPORT int IO_setOutput(char output, int value){
00597         try{
00598                 byte packet[32];
00599                 byte size;
00600                 packet[0] = 'T';
00601                 packet[1] = 'w';
00602                 packet[2] = (byte)value << output;
00603                 packet[3] = 0;
00604                 packet[4] = 0;
00605                 protocol->comm(packet, buffer, &size);
00606                 if (!buffer[0] || ((short) size != 2)) {
00607                         std::cout << " command failed!" << std::endl;
00608                         return ERR_FAILED;
00609                 }
00610         }
00611         catch(Exception &e){
00612                 std::cout << "ERROR: " << e.message() << std::endl;
00613                 #ifdef WIN32
00614                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00615                 #endif
00616                 return ERR_FAILED;
00617         }
00618         return ERR_SUCCESS;
00619 }
00621 int IO_readInput(int inputNr, int &value){
00622         try{
00623                 byte    packet[32];
00624                 byte size;
00625                 packet[0] = 'T';
00626                 packet[1] = 'r';
00627                 packet[2] = 0;
00628                 packet[3] = 0;
00629                 packet[4] = 0;
00630                 protocol->comm(packet, buffer, &size);
00631                 if (!buffer[0] || ((short) size != 2)) {
00632                         std::cout << " command failed!" << std::endl;
00633                         return ERR_FAILED;
00634                 }
00635         }
00636         catch(Exception &e){
00637                 std::cout << "ERROR: " << e.message() << std::endl;
00638                 #ifdef WIN32
00639                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00640                 #endif
00641                 return ERR_FAILED;
00642         }
00643         byte mask = static_cast<byte>(pow(static_cast<double>(2), inputNr-1));
00644         if(buffer[1] != 0xFF){
00645                 if((mask & (int)buffer[1]) > 0){
00646                         value = 0;
00647                 } else {
00648                         value = 1;
00649                 }
00650                 return ERR_SUCCESS;
00651         }
00652         else return ERR_FAILED;
00653 }
00655 DLLEXPORT int clearMoveBuffers(){
00656         try{
00657                 for(int i = 1; i <= getNumberOfMotors(); i++){
00658                         TMotTPS tps = { MCF_CLEAR_MOVEBUFFER, 0 };
00659                         katana->GetBase()->GetMOT()->arr[i].sendTPS(&tps);
00660                 }
00661         }
00662         catch(Exception &e){
00663                 std::cout << "ERROR: " << e.message() << std::endl;
00664                 #ifdef WIN32
00665                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00666                 #endif
00667                 return ERR_FAILED;
00668         }
00669         return ERR_SUCCESS;
00670 }
00672 int getVelocity(int axis, int &value){
00673         try{
00674                 katana->GetBase()->GetMOT()->arr[axis-1].recvPVP();
00675                 short vel = katana->GetBase()->GetMOT()->arr[axis-1].GetPVP()->vel;
00676                 value = (int) vel;
00677         }
00678         catch(Exception &e){
00679                 std::cout << "ERROR: " << e.message() << std::endl;
00680                 #ifdef WIN32
00681                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00682                 #endif
00683                 return ERR_FAILED;
00684         }
00685         return ERR_SUCCESS;
00686 }
00688 int getDrive(int axis, int &value){
00689         try{
00690                 katana->GetBase()->GetMOT()->arr[axis-1].recvPVP();
00691                 byte pwm = katana->GetBase()->GetMOT()->arr[axis-1].GetPVP()->pwm;
00692                 value = (int) pwm;
00693         }
00694         catch(Exception &e){
00695                 std::cout << "ERROR: " << e.message() << std::endl;
00696                 #ifdef WIN32
00697                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00698                 #endif
00699                 return ERR_FAILED;
00700         }
00701         return ERR_SUCCESS;
00702 }
00704 int getEncoder(int axis, int &value){
00705         try{
00706                 katana->getRobotEncoders(encoders.begin(), encoders.end());
00707                 value = encoders.at(axis-1);
00708         }
00709         catch(Exception &e){
00710                 std::cout << "ERROR: " << e.message() << std::endl;
00711                 #ifdef WIN32
00712                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00713                 #endif
00714                 return ERR_FAILED;
00715         }
00716         return ERR_SUCCESS;
00717 }
00719 int getAxisFirmwareVersion(int axis, char value[]){
00720         try{
00721                 katana->GetBase()->GetMOT()->arr[axis-1].recvSFW();
00722                 int v = (int) katana->GetBase()->GetMOT()->arr[axis-1].GetSFW()->version;
00723                 int sv = (int) katana->GetBase()->GetMOT()->arr[axis-1].GetSFW()->subversion;
00724                 int r = (int) katana->GetBase()->GetMOT()->arr[axis-1].GetSFW()->revision;
00725                 std::stringstream ss;
00726                 ss << v << "." << sv << "." << r;
00727                 const char* cstr = ss.str().c_str();
00728                 int index = -1;
00729                 do {
00730                         index++;
00731                         value[index] = cstr[index];
00732                 } while (cstr[index] != '\0');
00733         }
00734         catch(Exception &e){
00735                 std::cout << "ERROR: " << e.message() << std::endl;
00736                 #ifdef WIN32
00737                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00738                 #endif
00739                 return ERR_FAILED;
00740         }
00741         return ERR_SUCCESS;
00742 }
00744 int getVersion(char value[]){
00745         try{
00746                 katana->GetBase()->recvMFW();
00747                 int v = (int) katana->GetBase()->GetMFW()->ver;
00748                 int r = (int) katana->GetBase()->GetMFW()->rev;
00749                 std::stringstream ss;
00750                 ss << v << "." << r;
00751                 const char* cstr = ss.str().c_str();
00752                 int index = -1;
00753                 do {
00754                         index++;
00755                         value[index] = cstr[index];
00756                 } while (cstr[index] != '\0');
00757         }
00758         catch(Exception &e){
00759                 std::cout << "ERROR: " << e.message() << std::endl;
00760                 #ifdef WIN32
00761                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00762                 #endif
00763                 return ERR_FAILED;
00764         }
00765         return ERR_SUCCESS;
00766 }
00768 DLLEXPORT int setCollisionDetection(int axis, bool state){
00769         try{
00770                 if(state == true){
00771                         katana->enableCrashLimits();
00772                 }
00773                 else katana->disableCrashLimits();
00774         }
00775         catch(Exception &e){
00776                 std::cout << "ERROR: " << e.message() << std::endl;
00777                 #ifdef WIN32
00778                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00779                 #endif
00780                 return ERR_FAILED;
00781         }
00782         return ERR_SUCCESS;
00783 }
00785 DLLEXPORT int setPositionCollisionLimit(int axis, int limit){
00786         try{
00787                 katana->setPositionCollisionLimit(axis-1, limit);
00788         }
00789         catch(Exception &e){
00790                 std::cout << "ERROR: " << e.message() << std::endl;
00791                 #ifdef WIN32
00792                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00793                 #endif
00794                 return ERR_FAILED;
00795         }
00796         return ERR_SUCCESS;
00797 }
00799 DLLEXPORT int setVelocityCollisionLimit(int axis, int limit){
00800         try{
00801                 katana->setSpeedCollisionLimit(axis-1, limit);
00802         }
00803         catch(Exception &e){
00804                 std::cout << "ERROR: " << e.message() << std::endl;
00805                 #ifdef WIN32
00806                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00807                 #endif
00808                 return ERR_FAILED;
00809         }
00810         return ERR_SUCCESS;
00811 }
00813 DLLEXPORT int setCollisionParameters(int axis, int position, int velocity){
00814         try{
00815                 setPositionCollisionLimit(axis, position);
00816                 setVelocityCollisionLimit(axis, velocity);
00817         }
00818         catch(Exception &e){
00819                 std::cout << "ERROR: " << e.message() << std::endl;
00820                 #ifdef WIN32
00821                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00822                 #endif
00823                 return ERR_FAILED;
00824         }
00825         return ERR_SUCCESS;
00826 }
00828 DLLEXPORT int  setForceLimit(int axis, int limit){
00829         try{
00830                 katana->setForceLimit(axis, limit);
00831         }
00832         catch(Exception &e){
00833                 std::cout << "ERROR: " << e.message() << std::endl;
00834                 #ifdef WIN32
00835                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00836                 #endif
00837                 return ERR_FAILED;
00838         }
00839         return ERR_SUCCESS;
00840 }
00842 DLLEXPORT int  getForce(int axis){
00843         try{
00844                 return katana->getForce(axis);
00845         }
00846         catch(Exception &e){
00847                 std::cout << "ERROR: " << e.message() << std::endl;
00848                 #ifdef WIN32
00849                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00850                 #endif
00851                 return ERR_FAILED;
00852         }
00853 }
00855 DLLEXPORT int getCurrentControllerType(int axis){
00856         try{
00857                 return katana->getCurrentControllerType(axis);
00858         }
00859         catch(Exception &e){
00860                 std::cout << "ERROR: " << e.message() << std::endl;
00861                 #ifdef WIN32
00862                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00863                 #endif
00864                 return ERR_FAILED;
00865         }
00866 }
00868 DLLEXPORT int unblock(){
00869         try{
00870                 katana->unBlock();
00871         }
00872         catch(Exception &e){
00873                 std::cout << "ERROR: " << e.message() << std::endl;
00874                 #ifdef WIN32
00875                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00876                 #endif
00877                 return ERR_FAILED;
00878         }
00879         return ERR_SUCCESS;
00880 }
00882 DLLEXPORT int setControllerParameters(int axis, int ki, int kspeed, int kpos){
00883         try{
00884                 katana->GetBase()->GetMOT()->arr[axis-1].setControllerParameters(kspeed, kpos, ki);
00885         }
00886         catch(Exception &e){
00887                 std::cout << "ERROR: " << e.message() << std::endl;
00888                 #ifdef WIN32
00889                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00890                 #endif
00891                 return ERR_FAILED;
00892         }
00893         return ERR_SUCCESS;
00894 }
00896 DLLEXPORT int setMaxVelocity(int axis, int vel){
00897         try{
00898                 if (axis == 0){
00899                         for(int i = 0; i <= getNumberOfMotors()-1; i++){
00900                                 katana->setMotorVelocityLimit(i, vel);
00901                         }
00902                 }
00903                 else{
00904                         katana->setMotorVelocityLimit(axis-1, vel);
00905                 }
00906                 katana->setMaximumLinearVelocity((double)vel);
00907         }
00908         catch(Exception &e){
00909                 std::cout << "ERROR: " << e.message() << std::endl;
00910                 #ifdef WIN32
00911                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00912                 #endif
00913                 return ERR_FAILED;
00914         }
00915         return ERR_SUCCESS;
00916 }
00918 DLLEXPORT int setMaxAccel(int axis, int acceleration){
00919         try{
00920                 if (axis == 0){
00921                         for(int i = 0; i <= getNumberOfMotors()-1; i++){
00922                                 katana->setMotorAccelerationLimit(i, acceleration);
00923                         }
00924                 }
00925                 else{
00926                         katana->setMotorAccelerationLimit(axis-1, acceleration);
00927                 }
00928                 
00929         }
00930         catch(Exception &e){
00931                 std::cout << "ERROR: " << e.message() << std::endl;
00932                 #ifdef WIN32
00933                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00934                 #endif
00935                 return ERR_FAILED;
00936         }
00937         return ERR_SUCCESS;
00938 }
00940 DLLEXPORT int getNumberOfMotors(){
00941         try{
00942                 return katana->getNumberOfMotors();
00943         }
00944         catch(Exception &e){
00945                 std::cout << "ERROR: " << e.message() << std::endl;
00946                 #ifdef WIN32
00947                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00948                 #endif
00949                 return ERR_FAILED;
00950         }
00951         return ERR_SUCCESS;
00952 }
00954 DLLEXPORT int ping(int axis){
00955         try{
00956                 //throws ParameterReadingException if unsuccessful
00957                 katana->GetBase()->recvECH();
00958         }
00959         catch(Exception &e){
00960                 std::cout << "ERROR: " << e.message() << std::endl;
00961                 #ifdef WIN32
00962                         MessageBox(NULL, TEXT(e.message().c_str()),TEXT("KNI ERROR"),MB_OK | MB_ICONWARNING | MB_SYSTEMMODAL);
00963                 #endif
00964                 return ERR_FAILED;
00965         }
00966         return ERR_SUCCESS;
00967 }
00969 
00970 
00971 


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