kmlExt.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 
00022 #include "KNI/kmlExt.h"
00023 #include "KNI/kmlFactories.h"
00024 
00025 #include "common/MathHelperFunctions.h"
00026 #include "common/Timer.h"
00027 
00028 #include <iostream>
00029 #include <algorithm>
00030 #include <vector>
00031 
00032 #define max(a,b) (((a)>(b))?(a):(b))
00033 KNI::Timer kni_timer;
00034 
00036 const int POLLFREQUENCY = 300;
00037 
00038 void CKatana::inc(long idx, int dif, bool wait, int tolerance, long timeout) {
00039         base->GetMOT()->arr[idx].inc(dif,wait,tolerance,timeout);
00040 }
00041 
00042 void CKatana::dec(long idx, int dif, bool wait, int tolerance, long timeout) {
00043         base->GetMOT()->arr[idx].dec(dif,wait,tolerance,timeout);
00044 }
00045 
00046 void CKatana::mov(long idx, int tar, bool wait, int tolerance, long timeout) {
00047         base->GetMOT()->arr[idx].mov(tar, wait,tolerance,timeout);
00048 }
00049 
00050 
00051 
00052 void CKatana::incDegrees(long idx, double dif, bool wait, int tolerance, long timeout) {
00053         base->GetMOT()->arr[idx].incDegrees(dif,wait,tolerance,timeout);
00054 }
00055 
00056 void CKatana::decDegrees(long idx, double dif, bool wait, int tolerance, long timeout) {
00057         base->GetMOT()->arr[idx].decDegrees(dif,wait,tolerance,timeout);
00058 }
00059 
00060 void CKatana::movDegrees(long idx, double tar, bool wait, int tolerance, long timeout) {
00061         base->GetMOT()->arr[idx].movDegrees(tar,wait,tolerance,timeout);
00062 }
00063 
00064 
00065 void CKatana::create(const char* configurationFile, CCplBase* protocol) {
00066         KNI::kmlFactory infos;
00067         if(!infos.openFile(configurationFile)) {
00068                 throw ConfigFileOpenException(configurationFile);
00069         }
00070         create(&infos, protocol);
00071 }
00072 
00073 void CKatana::create(KNI::kmlFactory* infos, CCplBase* protocol) {
00074         base->init( infos->getGNL(), infos->getMOT(), infos->getSCT(), infos->getEFF(), protocol);
00075 
00076         for(int i=0; i<getNumberOfMotors(); ++i) {
00077                 TMotInit init = infos->getMotInit(i);
00078 
00079                 base->GetMOT()->arr[i].setInitialParameters(KNI_MHF::deg2rad(init.angleOffset), KNI_MHF::deg2rad(init.angleRange), init.encodersPerCycle, init.encoderOffset, init.rotationDirection);
00080 
00081                 TMotCLB clb = infos->getMotCLB(i);
00082                 base->GetMOT()->arr[i].setCalibrationParameters( clb.enable, clb.order, clb.dir, clb.mcf, clb.encoderPositionAfter );
00083 
00084                 base->GetMOT()->arr[i].setDYL( infos->getMotDYL(i) );
00085                 base->GetMOT()->arr[i].setSCP( infos->getMotSCP(i) );
00086         }
00087         mKatanaType = infos->getType();
00088         if (mKatanaType == 450) {
00089                 mKinematics = infos->getKinematics();
00090                 if (protocol != NULL)
00091                   base->flushMoveBuffers();
00092         } else {
00093                 mKinematics = 0;
00094         }
00095         if(base->checkKatanaType(mKatanaType) < 0){
00096                 //incompatible config file
00097                 std::cout << "\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
00098                 std::cout << "Exit: Incompatible Config File!\n";
00099                 std::cout << "Check whether you have a Katana 400 or 300 and choose the config file accordingly\n";
00100                 std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n.";
00101                 exit(0);
00102         }
00103         bool gripperIsPresent;
00104         int gripperOpenEncoders, gripperCloseEncoders;
00105         infos->getGripperParameters(gripperIsPresent, gripperOpenEncoders, gripperCloseEncoders);
00106         setGripperParameters(gripperIsPresent, gripperOpenEncoders, gripperCloseEncoders);
00107         
00108 }
00109 
00110 void CKatana::create(TKatGNL& gnl, TKatMOT& mot, TKatSCT& sct, TKatEFF& eff, CCplBase* protocol) {
00111         base->init(gnl, mot, sct, eff, protocol);
00112 }
00113 
00114 
00115 void CKatana::calibrate() {
00116         if(mKatanaType >= 400){
00117                 std::cout << "Katana4xx calibration started\n";
00118                 //standalone calibration for Katana400
00119                 TMotCmdFlg cflg = MCF_CALIB;
00120                 TMotTPS tps;
00121                 tps.tarpos = 32000;
00122                 tps.mcfTPS = cflg;
00123                 for(int i=0; i < getNumberOfMotors(); ++i){
00124                         base->GetMOT()->arr[i].setCalibrated(false);
00125                 }
00127                 //standalone calibration for K400:
00128                 byte    p[32];          //packet
00129                 byte    buf[256];       //readbuf
00130                 byte    sz = 10;                //readbuf size
00131                 p[0] = 'C';
00132                 p[1] = 0; //calibrate all motors
00133                 p[2] = 4; //calibrate
00134                 p[3] = (byte)(tps.tarpos >> 8);
00135                 p[4] = (byte)(tps.tarpos);
00136                 base->getProtocol()->comm(p,buf,&sz);
00138                 for (int i=0; i < getNumberOfMotors(); ++i) {
00139                         base->GetMOT()->arr[i].setCalibrated(true);
00140                 }
00141                 //wait for termination:
00142                 p[0] = 'D';
00143                 p[1] = 1;
00144                 do{
00145                         KNI::sleep(1000);
00146                         base->getProtocol()->comm(p,buf,&sz);
00147                 }
00148                 while(buf[2] == 4);
00149                 std::cout << "...done with calibration.\n";
00150                 
00151         }
00152         else if(mKatanaType == 300){
00153                 std::cout << "Katana300 calibration started\n";
00154                 KNI::sleep(500);
00155                 //----------------------------------------------------------------//
00156                 //"traditional" calibration, works only up to K400 V0.4.0
00157                 //For newer Katana types, "type" has to be set to 400 in the config file section [GENERAL]
00158                 //set motors ON before calibrating
00159                 //----------------------------------------------------------------//
00160                 TMotAPS aps;
00161                 for (int i=0; i<getNumberOfMotors(); i++) {
00162                         aps.actpos = 0;
00163                         aps.mcfAPS = MCF_ON;
00164                         base->GetMOT()->arr[i].sendAPS(&aps);
00165                 }
00166                 for (int i=0; i < getNumberOfMotors(); ++i) {
00167                         for(int j=0; j < getNumberOfMotors(); ++j) {
00168                                 if(base->GetMOT()->arr[j].GetCLB()->order == i) {
00169                                         base->GetMOT()->arr[j].setCalibrated(false);
00170                                         calibrate( j, *base->GetMOT()->arr[j].GetCLB(), *base->GetMOT()->arr[j].GetSCP(), *base->GetMOT()->arr[j].GetDYL() );
00171                                         base->GetMOT()->arr[j].setCalibrated(true);
00172                                         break;
00173                                 }
00174                         }
00175                 }
00176         }
00177 }
00178 
00179 
00180 
00181 void CKatana::calibrate(long idx, TMotCLB clb, TMotSCP scp, TMotDYL dyl) {
00182 
00183         if (!clb.enable)
00184                 return;
00185 
00186         searchMechStop(idx,clb.dir,scp,dyl);
00187 //std::cout << "setting actual position to " << base->GetMOT()->arr[idx].GetInitialParameters()->encoderOffset << " with motor command flag " << clb.mcf << std::endl;
00188         TMotAPS aps = { clb.mcf, static_cast<short int>(base->GetMOT()->arr[idx].GetInitialParameters()->encoderOffset) };
00189         base->GetMOT()->arr[idx].sendAPS(&aps);
00190 
00191         mov(idx, clb.encoderPositionAfter, true);
00192 }
00193 
00194 
00195 void CKatana::searchMechStop(long idx, TSearchDir dir,
00196                              TMotSCP _scp, TMotDYL _dyl ) {
00197 
00198         base->GetMOT()->arr[idx].setPwmLimits(_scp.maxppwm_nmp, _scp.maxnpwm_nmp);
00199         base->GetMOT()->arr[idx].setControllerParameters(_scp.kspeed_nmp, _scp.kpos_nmp, _scp.kI_nmp);
00200         base->GetMOT()->arr[idx].setCrashLimit(_scp.crash_limit_nmp);
00201         base->GetMOT()->arr[idx].setCrashLimitLinear(_scp.crash_limit_lin_nmp);
00202         base->GetMOT()->arr[idx].setAccelerationLimit(1);
00203         base->GetMOT()->arr[idx].setSpeedLimits(25, 25);
00204 
00205         TMotAPS aps;
00206         switch (dir) {
00207         case DIR_POSITIVE:
00208                 aps.actpos = -31000;            // Set the actual position equal to the
00209                 aps.mcfAPS = MCF_FREEZE;        // extreme opposite to direction I will move
00210                 base->GetMOT()->arr[idx].sendAPS(&aps);
00211                 break;
00212         case DIR_NEGATIVE:
00213                 aps.actpos = 31000;                     // Set the actual position equal to the
00214                 aps.mcfAPS = MCF_FREEZE;        // extreme opposite to direction I will move
00215                 base->GetMOT()->arr[idx].sendAPS(&aps);
00216                 break;
00217         };
00218 
00219         TMotTPS tps;
00220         switch (dir) {
00221         case DIR_POSITIVE:
00222                 tps.tarpos = 32000;
00223                 tps.mcfTPS = MCF_ON;
00224                 base->GetMOT()->arr[idx].sendTPS(&tps); // Set the target position equal to
00225                 // the extreme I am moving towards.
00226                 break;
00227         case DIR_NEGATIVE:
00228                 tps.tarpos = -32000;
00229                 tps.mcfTPS = MCF_ON;
00230                 base->GetMOT()->arr[idx].sendTPS(&tps);// Set the target position equal to
00231                 // the extreme I am moving towards.
00232 
00233                 break;
00234         };
00235 
00236         double firstSpeedSample = 100, secondSpeedSample = 100;
00237 
00238         KNI::Timer poll_t(POLLFREQUENCY);
00239         while(true) {
00240                 poll_t.Start();
00241                 base->GetMOT()->arr[idx].recvPVP();
00242                 firstSpeedSample = base->GetMOT()->arr[idx].GetPVP()->vel;
00243                 //std::cout << firstSpeedSample << ", " << secondSpeedSample << std::endl;
00244                 if( (firstSpeedSample + secondSpeedSample) == 0.0 ) {
00245                         break; // stopper reached
00246                 }
00247                 secondSpeedSample = firstSpeedSample;
00248                 poll_t.WaitUntilElapsed();
00249         }
00250         // To avoid a compensation on the motor the actual position is set to 0
00251         // Otherwise, as it didn't reach the target position it could attempt to go
00252         // on moving
00253 //std::cout << "V=0 @: " << base->GetMOT()->arr[idx].GetPVP()->pos << std::endl;
00254         aps.actpos = 0;
00255         aps.mcfAPS = MCF_FREEZE;
00256 //std::cout << "actual pos struct set to pos 0 and motorcommandflag 8 (hold robot)\nsending actual pos..." << std::endl;
00257         base->GetMOT()->arr[idx].sendAPS(&aps);
00258 //std::cout << "sent successfully." << std::endl;
00259 
00260         // restore motor parameters
00261         base->GetMOT()->arr[idx].setPwmLimits(_scp.maxppwm_nmp, _scp.maxnpwm_nmp);
00262         base->GetMOT()->arr[idx].setControllerParameters(_scp.kspeed_nmp, _scp.kpos_nmp, _scp.kI_nmp);
00263         base->GetMOT()->arr[idx].setCrashLimit(_scp.crash_limit_nmp);
00264         base->GetMOT()->arr[idx].setCrashLimitLinear(_scp.crash_limit_lin_nmp);
00265         base->GetMOT()->arr[idx].setAccelerationLimit((short) _dyl.maxaccel_nmp);
00266         base->GetMOT()->arr[idx].setSpeedLimits(_dyl.maxpspeed_nmp, _dyl.maxnspeed_nmp);
00267 
00268 }
00269 
00270 
00271 void CKatana::setTolerance(long idx, int enc_tolerance) {
00272         base->GetMOT()->arr[idx].setTolerance(enc_tolerance);
00273 }
00274 
00275 
00276 bool CKatana::checkENLD(long idx, double degrees) {
00277         return base->GetMOT()->arr[idx].checkAngleInRange(degrees);
00278 }
00279 
00280 
00281 void CKatana::enableCrashLimits() {
00282         base->enableCrashLimits();
00283 }
00284 
00285 
00286 void CKatana::disableCrashLimits() {
00287         base->disableCrashLimits();
00288 }
00289 
00290 
00291 void CKatana::unBlock() {
00292         base->unBlock();
00293 }
00294 
00295 
00296 void CKatana::flushMoveBuffers() {
00297         base->flushMoveBuffers();
00298 }
00299 
00300 
00301 void CKatana::setCrashLimit(long idx, int limit) {
00302         base->setCrashLimit(idx, limit);
00303 }
00304 
00306 void CKatana::setPositionCollisionLimit(long idx, int limit){
00307         base->GetMOT()->arr[idx].setPositionCollisionLimit(limit);
00308 }
00310 void CKatana::setSpeedCollisionLimit(long idx, int limit){
00311         base->GetMOT()->arr[idx].setSpeedCollisionLimit(limit);
00312 }
00314 void CKatana::setForceLimit(int axis, int limit){
00315         if(axis == 0){
00316                 for (int i=1; i <= getNumberOfMotors(); i++) {
00317                         setForceLimit(i, limit);
00318                 }
00319         }
00320         
00321         // check axis number
00322         if (axis < 1) return;
00323         if (axis > getNumberOfMotors()) return;
00324         
00325         // can not set current limit on drive controller
00326         if (base->GetMOT()->arr[axis-1].GetSFW()->type == 0) return;
00327         
00328         int force = abs(limit);
00329         if (force > 100)
00330                 force = 100;
00331 
00332         byte    p[32];          //packet
00333         byte    buf[256];       //readbuf
00334         byte    sz = 0;         //readbuf size
00335         p[0] = 'S';
00336         p[1] = axis;
00337         p[2] = 10;              // subcommand 10 "Set Current Controller Limit"
00338         p[3] = (char)(force >> 8);
00339         p[4] = (char)(force);
00340         p[5] = 0;
00341         base->getProtocol()->comm(p,buf,&sz);
00342 }
00344 short CKatana::getForce(int axis){
00345         byte r1, r2, r3;
00346         base->GetMOT()->arr[axis-1].getParameterOrLimit(244, &r1, &r2, &r3);
00347         char force = (char)r2;
00348         return (short)force;
00349 }
00351 int CKatana::getCurrentControllerType(int axis){
00352         base->GetMOT()->arr[axis-1].recvSFW();
00353         return (int) base->GetMOT()->arr[axis-1].GetSFW()->type;
00354 }
00356 short
00357 CKatana::getNumberOfMotors() const {
00358         return base->GetMOT()->cnt;
00359 }
00360 
00361 int
00362 CKatana::getMotorEncoders(short number, bool refreshEncoders) const {
00363         if(refreshEncoders)
00364                 base->GetMOT()->arr[number].recvPVP(); // error handling using exceptions
00365         return base->GetMOT()->arr[number].GetPVP()->pos;
00366 }
00367 
00368 std::vector<int>::iterator
00369 CKatana::getRobotEncoders(std::vector<int>::iterator start, std::vector<int>::const_iterator end, bool refreshEncoders) const {
00370         if(refreshEncoders)
00371                 base->recvMPS();
00372         std::vector<int>::iterator iter = start;
00373         for(int i = 0; i < getNumberOfMotors(); ++i) {
00374                 if(iter == end)
00375                         return iter;
00376                 (*iter) = getMotorEncoders(i, false);
00377                 ++iter;
00378         }
00379         return iter;
00380 }
00381 
00382 std::vector<int>
00383 CKatana::getRobotEncoders(bool refreshEncoders) const {
00384         std::vector<int> temp(getNumberOfMotors());
00385         getRobotEncoders(temp.begin(),temp.end(), refreshEncoders);
00386         return temp;
00387 }
00388 
00389 short
00390 CKatana::getMotorVelocityLimit(short number) const {
00391         return base->GetMOT()->arr[number].GetDYL()->maxpspeed_nmp;
00392 }
00393 short
00394 CKatana::getMotorAccelerationLimit(short number) const {
00395         return base->GetMOT()->arr[number].GetDYL()->maxaccel_nmp;
00396 }
00397 
00398 void
00399 CKatana::setMotorVelocityLimit(short number, short velocity) {
00400         base->GetMOT()->arr[number].setSpeedLimit(velocity);
00401 }
00402 
00403 void
00404 CKatana::setRobotVelocityLimit(short velocity) {
00405         for(short c = 0; c < getNumberOfMotors(); ++c) {
00406                 base->GetMOT()->arr[c].setSpeedLimit( velocity );
00407         }
00408 }
00409 
00410 void
00411 CKatana::setMotorAccelerationLimit(short number, short acceleration) {
00412         base->GetMOT()->arr[number].setAccelerationLimit(acceleration);
00413 }
00414 
00415 void
00416 CKatana::setRobotAccelerationLimit(short acceleration) {
00417         for(short c = 0; c < getNumberOfMotors(); ++c) {
00418                 base->GetMOT()->arr[c].setAccelerationLimit(acceleration);
00419         }
00420 }
00421 
00422 void
00423 CKatana::moveMotorByEnc(short number, int encoders, bool waitUntilReached, int waitTimeout) {
00424         if(encoders >= 0) {
00425                 inc(number, encoders, waitUntilReached, waitTimeout);
00426         } else {
00427                 dec(number, abs(encoders), waitUntilReached, 100, waitTimeout);
00428         }
00429 }
00430 
00431 void
00432 CKatana::moveMotorBy(short number, double radianAngle, bool waitUntilReached, int waitTimeout) {
00433         double degree = radianAngle/M_PI*180;
00434         base->GetMOT()->arr[number].incDegrees(degree, waitUntilReached, 100, waitTimeout);
00435 }
00436 
00437 void
00438 CKatana::moveMotorToEnc(short number, int encoders, bool waitUntilReached, int encTolerance, int waitTimeout) {
00439         mov(number, encoders, waitUntilReached, encTolerance, waitTimeout);
00440 }
00441 
00442 void
00443 CKatana::moveMotorTo(short number, double radianAngle, bool waitUntilReached, int waitTimeout) {
00444         int encoders = KNI_MHF::rad2enc( radianAngle,
00445                                          base->GetMOT()->arr[number].GetInitialParameters()->angleOffset,
00446                                          base->GetMOT()->arr[number].GetInitialParameters()->encodersPerCycle,
00447                                          base->GetMOT()->arr[number].GetInitialParameters()->encoderOffset,
00448                                          base->GetMOT()->arr[number].GetInitialParameters()->rotationDirection);
00449         mov(number, encoders, waitUntilReached, 100, waitTimeout);
00450 }
00451 
00452 void
00453 CKatana::waitForMotor( short number, int encoders, int encTolerance, short mode, int waitTimeout){
00454         base->GetMOT()->arr[number].waitForMotor(encoders,encTolerance, mode, waitTimeout);
00455 }
00456 
00457 void
00458 CKatana::waitFor(TMotStsFlg status, int waitTimeout) {
00459         base->waitFor(status, waitTimeout, _gripperIsPresent);
00460 }
00461 
00462 void
00463 CKatana::moveRobotToEnc(std::vector<int>::const_iterator start, std::vector<int>::const_iterator end, bool waitUntilReached, int encTolerance, int waitTimeout) {
00464 
00465         //         // We'll do that some other time. We need to store the velocity twice for that
00466         //         for(unsigned int i = 0; i < getNumberOfMotors(); ++i) {
00467         //             distance[i] = std::abs(act_pos[i] - solution[i]);
00468         //         }
00469         //         int maxDistNr = std::distance(distance.begin(), std::max_element(distance.begin(), distance.end()));
00470 
00471         int i = 0;
00472         std::vector<int>::const_iterator iter = start;
00473         while( (iter != end) && (i < getNumberOfMotors()) ) {
00474                 //             if(i != maxDistNr) {
00475                 //                 mot->arr[i].setSpeedLimit(distance[i]/(distance[maxDistNr]/mot->arr[maxDistNr].GetDYL()->maxpspeed_nmp));
00476                 //             }
00477                 mov(i, *iter, false, encTolerance, waitTimeout);
00478                 ++i;
00479                 ++iter;
00480         }
00481 
00482         // If wait is true, check if the target position is reached
00483         if(!waitUntilReached)
00484                 return;
00485 
00486         const TKatMOT* mot = base->GetMOT();
00487         bool pos_reached;
00488         KNI::Timer t(waitTimeout), poll_t(POLLFREQUENCY);
00489         t.Start();
00490         while (true) {
00491                 if (t.Elapsed())
00492                         throw MotorTimeoutException();
00493                 pos_reached = true;
00494                 poll_t.Start();
00495                 base->recvMPS(); // get position for all motors
00496                 base->recvGMS(); // get status flags for all motors
00497                 for (int idx=0; idx<getNumberOfMotors(); idx++) {
00498                         if (mot->arr[idx].GetPVP()->msf == 40)
00499                                 throw MotorCrashException();
00500                         pos_reached &= std::abs(mot->arr[idx].GetTPS()->tarpos - mot->arr[idx].GetPVP()->pos) < 100;
00501                 }
00502                 if (pos_reached)
00503                         break;
00504                 poll_t.WaitUntilElapsed();
00505         }
00506 }
00507 
00508 void
00509 CKatana::moveRobotToEnc(std::vector<int> encoders, bool waitUntilReached, int encTolerance, int waitTimeout) {
00510         moveRobotToEnc(encoders.begin(), encoders.end(), waitUntilReached, encTolerance, waitTimeout);
00511 }
00512 
00513 void
00514 CKatana::moveRobotToEnc4D(std::vector<int> target, int velocity, int acceleration, int encTolerance){
00515 
00516         int n, maxDistance = 0;
00517         short numberOfMotors = getNumberOfMotors();
00518     std::vector<int> diffMot,speed,oldSpeed;
00519 
00520         //Find the maximun difference between actual and target position for each motor
00521         for (n=0;n<numberOfMotors;n++){
00522                 diffMot.push_back(std::abs(getMotorEncoders(n,true)-target.at(n)));
00523                 maxDistance=max(diffMot.at(n),maxDistance);
00524         }
00525 
00526         //Save the old speeds and calculate the new speeds
00527         for (n=0;n<numberOfMotors;n++){
00528              oldSpeed.push_back(getMotorVelocityLimit(n));
00529              speed.push_back(max(static_cast<int>((static_cast<double>(diffMot.at(n))/maxDistance) * velocity),10));
00530              setMotorVelocityLimit(n,speed.at(n));
00531              setMotorAccelerationLimit(n,acceleration);
00532         }
00533 
00534         //Move each motor to the target position
00535         for (n=0;n<numberOfMotors;n++){
00536                 moveMotorToEnc(n,target.at(n));
00537         }
00538 
00539         //Wait until the target position for all motors with the encTolerance is reached
00540         for (n=0;n<numberOfMotors;n++){
00541                 waitForMotor(n,target.at(n),encTolerance);
00542         }
00543 
00544         //Restore the speeds
00545         for (n=0;n<numberOfMotors;n++){
00546                 setMotorVelocityLimit(n,oldSpeed.at(n));
00547         }
00548 }
00549 
00550 void
00551 CKatana::openGripper(bool waitUntilReached, int waitTimeout) {
00552         if(!_gripperIsPresent)
00553                 return;
00554         moveMotorToEnc( getNumberOfMotors()-1, _gripperOpenEncoders, waitUntilReached, waitTimeout );
00555 }
00556 
00557 void
00558 CKatana::closeGripper(bool waitUntilReached, int waitTimeout) {
00559         if(!_gripperIsPresent)
00560                 return;
00561         moveMotorToEnc( getNumberOfMotors()-1, _gripperCloseEncoders, waitUntilReached, waitTimeout );
00562 }
00563 
00564 
00565 void
00566 CKatana::freezeRobot() {
00567         for(int i = 0; i < getNumberOfMotors(); ++i)
00568                 freezeMotor(i);
00569 }
00570 void
00571 CKatana::freezeMotor(short number) {
00572         base->GetMOT()->arr[number].recvPVP();
00573         const TMotPVP *pvp = base->GetMOT()->arr[number].GetPVP();
00574         TMotTPS tps = { MCF_FREEZE, pvp->pos };
00575         base->GetMOT()->arr[number].sendTPS(&tps);
00576 }
00577 void
00578 CKatana::switchRobotOn() {
00579         for(int i = 0; i < getNumberOfMotors(); ++i)
00580                 // switchMotorOn(i); // with moving flag, old version for katana 1.1
00581                 freezeMotor(i); // switch on with freeze flag, new version, safer and for katana 1.2 too
00582 }
00583 void
00584 CKatana::switchRobotOff() {
00585         for(int i = 0; i < getNumberOfMotors(); ++i)
00586                 switchMotorOff(i);
00587 }
00588 void
00589 CKatana::switchMotorOn(short number) {
00590         base->GetMOT()->arr[number].recvPVP();
00591         const TMotPVP *pvp = base->GetMOT()->arr[number].GetPVP();
00592         TMotTPS tps = { MCF_FREEZE, pvp->pos };
00593         base->GetMOT()->arr[number].sendTPS(&tps);
00594 }
00595 void
00596 CKatana::switchMotorOff(short number) {
00597         base->GetMOT()->arr[number].recvPVP();
00598         const TMotPVP *pvp = base->GetMOT()->arr[number].GetPVP();
00599         TMotTPS tps = { MCF_OFF, pvp->pos };
00600         base->GetMOT()->arr[number].sendTPS(&tps);
00601 }
00602 
00603 void
00604 CKatana::setGripperParameters(bool isPresent, int openEncoders, int closeEncoders) {
00605         _gripperIsPresent     = isPresent;
00606         _gripperOpenEncoders  = openEncoders;
00607         _gripperCloseEncoders = closeEncoders;
00608 }
00609 
00610 void
00611 CKatana::getGripperParameters(bool &isPresent, int &openEncoders, int &closeEncoders) {
00612         isPresent = _gripperIsPresent;
00613         openEncoders = _gripperOpenEncoders;
00614         closeEncoders = _gripperCloseEncoders;
00615 }
00616 
00617 void
00618 CKatana::startSplineMovement(bool exactflag, int moreflag) {
00619         int exact = 0;
00620         if (exactflag) {
00621                 exact = 1;
00622         }
00623         if (!_gripperIsPresent) {
00624                 exact += 2;
00625         }
00626     base->startSplineMovement(exact, moreflag);
00627 }
00628 
00629 
00630 void
00631 CKatana::sendSplineToMotor(short number, short targetPosition, short duration, short p1, short p2, short p3, short p4) {
00632     base->GetMOT()->arr[number].sendSpline(targetPosition, duration, p1, p2, p3, p4);
00633 }
00634 
00635 void
00636 CKatana::setAndStartPolyMovement(std::vector<short> polynomial, bool exactflag, int moreflag) {
00637         int exact = 0;
00638         if (exactflag) {
00639                 exact = 1;
00640         }
00641         if (!_gripperIsPresent) {
00642                 exact += 2;
00643         }
00644         base->setAndStartPolyMovement(polynomial, exact, moreflag);
00645 }
00646 
00647 int CKatana::readDigitalIO(){
00648         return base->readDigitalIO();
00649 }


kni
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:44:12