kmlMotBase.cpp
Go to the documentation of this file.
00001 //
00002 // C++ Implementation: kmlMotBase
00003 //
00004 // Description:
00005 //
00006 //
00007 // Author: Tiziano Müller <tiziano.mueller@neuronics.ch>, (C) 2006
00008 //
00009 // Copyright: See COPYING file that comes with this distribution
00010 //
00011 //
00012 
00013 #include "KNI/kmlMotBase.h"
00014 
00015 #include "common/MathHelperFunctions.h"
00016 #include "common/Timer.h"
00017 #include <iostream>
00018 
00019 
00020 bool CMotBase::init(CKatBase* _own, const TMotDesc _motDesc, CCplBase* _protocol) {
00021         gnl.own = _own;
00022         gnl.SID = _motDesc.slvID;
00023         protocol =  _protocol;
00024         try {
00025                 if (protocol != NULL)
00026                   recvSFW();
00027         } catch (ParameterReadingException pre) {
00028                 sfw.type = 0; // set position controller for now
00029                 return true;
00030         }
00031         return true;
00032 }
00033 
00034 
00035 void CMotBase::resetBlocked() {
00036         byte    p[32];          //packet
00037         byte    buf[256];       //readbuf
00038         byte    sz = 0;         //readbuf size
00039 
00040         recvPVP();
00041 
00042         p[0] = 'C';
00043         p[1] = gnl.SID;
00044         p[2] = MCF_FREEZE;                      // Flag to freeze
00045         p[3] = (byte)(GetPVP()->pos >> 8);
00046         p[4] = (byte)(GetPVP()->pos);
00047 
00048         protocol->comm(p,buf,&sz);
00049 
00050         aps.mcfAPS = MCF_FREEZE;
00051 }
00052 
00053 void CMotBase::sendAPS(const TMotAPS* _aps) {
00054         byte    p[32];          //packet
00055         byte    buf[256];       //readbuf
00056         byte    sz = 0;         //readbuf size
00057 
00058         p[0] = 'C';
00059         p[1] = gnl.SID + 128;
00060         p[2] = _aps->mcfAPS;
00061         p[3] = (byte)(_aps->actpos >> 8);
00062         p[4] = (byte)(_aps->actpos);
00063 
00064         protocol->comm(p,buf,&sz);
00065 
00066         if (!buf[1])
00067                 throw ParameterWritingException("APS");
00068 
00069         aps = *_aps;
00070 
00071 }
00072 
00073 void CMotBase::sendTPS(const TMotTPS* _tps) {
00074         byte    p[32];          //packet
00075         byte    buf[256];       //readbuf
00076         byte    sz = 0;         //readbuf size
00077 
00078         p[0] = 'C';
00079         p[1] = gnl.SID;
00080         p[2] = _tps->mcfTPS;
00081         p[3] = (byte)(_tps->tarpos >> 8);
00082         p[4] = (byte)(_tps->tarpos);
00083 
00084         protocol->comm(p,buf,&sz);
00085         if (!buf[1])
00086                 throw ParameterWritingException("TPS");
00087         tps = *_tps;
00088 }
00089 
00090 void CMotBase::recvPVP() {
00091 
00092         byte    p[32];          //packet
00093         byte    buf[256];       //readbuf
00094         byte    sz = 0;         //readbuf size
00095 
00096         p[0] = 'D';
00097         p[1] = gnl.SID;
00098 
00099         protocol->comm(p,buf,&sz);
00100         if (!buf[1])
00101                 throw ParameterReadingException("PVP");
00102         pvp.msf         = (TMotStsFlg)buf[2];
00103         pvp.pos         = (((short)buf[3])<<8) | buf[4];
00104         pvp.vel         = (((short)buf[5])<<8) | buf[6];
00105         pvp.pwm         = buf[7];
00106 
00107 }
00108 
00109 void CMotBase::recvSFW() {
00110         byte    p[32];          //packet
00111         byte    buf[256];       //readbuf
00112         byte    sz = 0;         //readbuf size
00113 
00114         p[0] = 'V';
00115         p[1] = gnl.SID;
00116         p[2] = 32;
00117 
00118         protocol->comm(p,buf,&sz);
00119         if (!buf[1])
00120                 throw ParameterReadingException("SFW");
00121         sfw.version     = buf[3];
00122         sfw.subversion  = buf[4];
00123         sfw.revision    = buf[5];
00124         sfw.type        = buf[6];
00125         sfw.subtype     = buf[7];
00126 
00127 }
00128 
00129 
00130 void CMotBase::setSpeedLimits(short positiveVelocity, short negativeVelocity) {
00131 
00132         byte p[32];             //packet
00133         byte buf[256];  //readbuf
00134         byte sz = 0;    //readbuf size
00135         p[0] = 'S';
00136         p[1] = gnl.SID;
00137         p[2] = 3;               // subcommand 3 "Set Speed Limits"
00138         p[3] = static_cast<byte>(positiveVelocity);
00139         p[4] = static_cast<byte>(negativeVelocity);
00140         p[5] = 0;
00141 
00142         protocol->comm(p,buf,&sz);
00143 
00144         dyl.maxnspeed = dyl.maxnspeed_nmp = negativeVelocity;
00145         dyl.maxpspeed = dyl.maxpspeed_nmp = positiveVelocity;
00146 
00147 }
00148 
00149 void CMotBase::setAccelerationLimit( short acceleration ) {
00150 
00151         byte p[32];             //packet
00152         byte buf[256];  //readbuf
00153         byte sz = 0;    //readbuf size
00154         p[0] = 'S';
00155         p[1] = gnl.SID;
00156         p[2] = 4;               // subcommand 4 "Set Acceleration Limit"
00157         p[3] = static_cast<byte>(acceleration);
00158         p[4] = 0;
00159         p[5] = 0;
00160 
00161         protocol->comm(p,buf,&sz);
00162         dyl.maxaccel_nmp = dyl.maxaccel = static_cast<byte>(acceleration);
00163 }
00164 
00165 void CMotBase::setPwmLimits(byte maxppwm, byte maxnpwm) {
00166 
00167         if (sfw.type == 1) return; // can not set pwm limit on current controller
00168         
00169         byte p[32];             //packet
00170         byte buf[256];  //readbuf
00171         byte sz = 0;    //readbuf size
00172         p[0] = 'S';
00173         p[1] = gnl.SID;
00174         p[2] = 2;               // subcommand 2 "Set PWM Limits"
00175         p[3] = maxppwm;
00176         p[4] = maxnpwm;
00177         p[5] = 0;
00178 
00179         protocol->comm(p,buf,&sz);
00180         scp.maxppwm_nmp = scp.maxppwm = maxppwm;
00181         scp.maxnpwm_nmp = scp.maxnpwm = maxnpwm;
00182 
00183         return;
00184 }
00185 
00186 void CMotBase::setControllerParameters(byte kSpeed, byte kPos, byte kI) {
00187 
00188         byte p[32];             //packet
00189         byte buf[256];  //readbuf
00190         byte sz = 0;    //readbuf size
00191         p[0] = 'S';
00192         p[1] = gnl.SID;
00193         p[2] = 1;               // subcommand 1 "Set Controller Parameters"
00194         p[3] = kSpeed;
00195         p[4] = kPos;
00196         p[5] = kI;
00197 
00198         protocol->comm(p,buf,&sz);
00199         scp.kspeed_nmp = scp.kP_speed = kSpeed;
00200         scp.kpos_nmp = scp.kP = kPos;
00201         scp.kI_nmp = kI; // no corresponding old motor parameter
00202 
00203         return;
00204 }
00205 
00206 void CMotBase::setCrashLimit(int limit) {
00207 
00208         byte p[32];             //packet
00209         byte buf[256];  //readbuf
00210         byte sz = 0;    //readbuf size
00211         p[0] = 'S';
00212         p[1] = gnl.SID;
00213         p[2] = 5;               // subcommand 5 "Set Crash Limit"
00214         p[3] = (byte) (limit >> 8);
00215         p[4] = (byte) limit;
00216         p[5] = 0;
00217 
00218         protocol->comm(p,buf,&sz);
00219         scp.crash_limit_nmp = limit;
00220 
00221         return;
00222 }
00223 
00224 void CMotBase::setCrashLimitLinear(int limit_lin) {
00225 
00226         byte p[32];             //packet
00227         byte buf[256];  //readbuf
00228         byte sz = 0;    //readbuf size
00229         p[0] = 'S';
00230         p[1] = gnl.SID;
00231         p[2] = 6;               // subcommand 6 "Set Crash Limit Linear"
00232         p[3] = (byte) (limit_lin >> 8);
00233         p[4] = (byte) limit_lin;
00234         p[5] = 0;
00235 
00236         protocol->comm(p,buf,&sz);
00237         scp.crash_limit_lin_nmp = limit_lin;
00238 
00239         return;
00240 }
00242 //for Katana400s:
00243 void CMotBase::setSpeedCollisionLimit(int limit){
00244         byte p[32];             
00245         byte buf[256];  
00246         byte sz = 0;    
00247         p[0] = 'S';
00248         p[1] = gnl.SID;
00249         p[2] = 7;               
00250         p[3] = (byte) limit; //for both the linear and non-linear the same:
00251         p[4] = (byte) limit;
00252         p[5] = 0;
00253         protocol->comm(p,buf,&sz);
00254         scp.crash_limit_nmp = limit;
00255         return;
00256 }
00258 //for Katana400s:
00259 void CMotBase::setPositionCollisionLimit(int limit){
00260         byte p[32];             
00261         byte buf[256];  
00262         byte sz = 0;    
00263         p[0] = 'S';
00264         p[1] = gnl.SID;
00265         p[2] = 5;               
00266         p[3] = (byte) (limit >> 8);
00267         p[4] = (byte) limit;
00268         p[5] = 0;
00269         protocol->comm(p,buf,&sz);
00270         scp.crash_limit_nmp = limit;
00271         return;
00272 }
00273 
00274 void CMotBase::getParameterOrLimit(int subcommand, byte* R1, byte* R2, byte* R3) {
00275         // illegal subcommand
00276         if ((subcommand > 255) || (subcommand < 240)) {
00277                 *R1 = 0;
00278                 *R2 = 0;
00279                 *R3 = 0;
00280                 return;
00281         }
00282         byte p[32];             //packet
00283         byte buf[256];  //readbuf
00284         byte sz = 0;    //readbuf size
00285         p[0] = 'S';
00286         p[1] = gnl.SID;
00287         p[2] = (byte) subcommand;
00288         p[3] = 0;
00289         p[4] = 0;
00290         p[5] = 0;
00291         protocol->comm(p,buf,&sz);
00292         *R1 = buf[3];
00293         *R2 = buf[4];
00294         *R3 = buf[5];
00295 
00296         return;
00297 }
00298 
00299 void CMotBase::sendSpline(short targetPosition, short duration, short p1, short p2, short p3, short p4) {
00300     std::vector<byte> sendBuf(14), recvBuf(2, 0);
00301         byte readBytes = 0;
00302 
00303         sendBuf[0] = 'G';
00304         sendBuf[1] = gnl.SID;
00305     sendBuf[2] = static_cast<byte>(targetPosition >> 8);
00306     sendBuf[3] = static_cast<byte>(targetPosition);
00307     sendBuf[4] = static_cast<byte>(duration >> 8);
00308     sendBuf[5] = static_cast<byte>(duration);
00309 
00310     sendBuf[6] = static_cast<byte>(p1 >> 8);
00311     sendBuf[7] = static_cast<byte>(p1);
00312     sendBuf[8] = static_cast<byte>(p2 >> 8);
00313     sendBuf[9] = static_cast<byte>(p2);
00314     sendBuf[10] = static_cast<byte>(p3 >> 8);
00315     sendBuf[11] = static_cast<byte>(p3);
00316     sendBuf[12] = static_cast<byte>(p4 >> 8);
00317     sendBuf[13] = static_cast<byte>(p4);
00318 
00319         protocol->comm(&sendBuf.front(), &recvBuf.front(), &readBytes);
00320 }
00321 
00322 
00323 void
00324 CMotBase::setInitialParameters(double angleOffset, double angleRange, int encodersPerCycle, int encoderOffset, int rotationDirection) {
00325 
00326         _initialParameters.angleOffset = angleOffset;
00327         _initialParameters.angleRange = angleRange;
00328         _initialParameters.encoderOffset = encoderOffset;
00329         _initialParameters.encodersPerCycle = encodersPerCycle;
00330         _initialParameters.rotationDirection = rotationDirection;
00331 
00332         _initialParameters.angleStop = angleOffset + angleRange;
00333 
00334         int encoderStop = encoderOffset - rotationDirection*static_cast<int>(encodersPerCycle*(angleRange/(2.0*M_PI)));
00335 
00336         _encoderLimits.enc_minpos = (encoderOffset > encoderStop) ? encoderStop : encoderOffset;
00337         _encoderLimits.enc_maxpos = (encoderOffset < encoderStop) ? encoderStop : encoderOffset;
00338         _encoderLimits.enc_per_cycle = encodersPerCycle;
00339         _encoderLimits.enc_range = std::abs(_encoderLimits.enc_minpos - _encoderLimits.enc_maxpos);
00340 }
00341 
00342 void
00343 CMotBase::setCalibrationParameters(bool doCalibration, short order, TSearchDir direction, TMotCmdFlg motorFlagAfter, int encoderPositionAfter) {
00344         _calibrationParameters.enable = doCalibration;
00345         _calibrationParameters.order  = order;
00346         _calibrationParameters.dir    = direction;
00347         _calibrationParameters.mcf    = motorFlagAfter;
00348         _calibrationParameters.encoderPositionAfter = encoderPositionAfter;
00349         _calibrationParameters.isCalibrated = false;
00350 }
00351 
00352 void
00353 CMotBase::setCalibrated(bool calibrated) {
00354         _calibrationParameters.isCalibrated = calibrated;
00355 }
00356 
00357 void
00358 CMotBase::setTolerance(int tolerance) {
00359         _encoderLimits.enc_tolerance = tolerance;
00360 }
00361 
00362 bool CMotBase::checkAngleInRange(double angle) {
00363         return (angle >= _initialParameters.angleOffset) && (angle <= _initialParameters.angleStop);
00364 }
00365 bool CMotBase::checkEncoderInRange(int encoder) {
00366         return (encoder >= _encoderLimits.enc_minpos) && (encoder <= _encoderLimits.enc_maxpos);
00367 }
00368 
00369 
00370 void CMotBase::inc(int dif, bool wait, int tolerance, long timeout) {
00371         recvPVP();
00372         mov( GetPVP()->pos + dif, wait, tolerance, timeout);
00373 }
00374 
00375 void CMotBase::dec(int dif, bool wait, int tolerance, long timeout) {
00376         recvPVP();
00377         mov(GetPVP()->pos - dif, wait, tolerance, timeout);
00378 }
00379 
00380 void CMotBase::mov(int tar, bool wait, int tolerance, long timeout) {
00381 
00382         if (!checkEncoderInRange(tar))
00383                 throw MotorOutOfRangeException();
00384 
00385         tps.mcfTPS = MCF_ON;
00386         tps.tarpos = tar;
00387 
00388         sendTPS(&tps);
00389 
00390         if (wait)
00391                 waitForMotor(tar,tolerance,0,timeout);
00392         else
00393                 return;
00394 }
00395 
00396 void CMotBase::waitForMotor(int target, int encTolerance, short mode,
00397                 int waitTimeout) {
00398         const long POLLFREQUENCY = 200;
00399         KNI::Timer t(waitTimeout), poll_t(POLLFREQUENCY);
00400         t.Start();
00401         while (true) {
00402                 if (t.Elapsed())
00403                         throw MotorTimeoutException();
00404                 poll_t.Start();
00405                 recvPVP();
00406                 if (GetPVP()->msf == 40)
00407                         throw MotorCrashException();
00408                 switch(mode)
00409                 {
00410                         case 0:
00411                                 if (std::abs(target - GetPVP()->pos) < encTolerance)
00412                                         return; // position reached
00413                                 break;
00414                         case 1:
00415                                 if (GetPVP()->msf == MSF_DESPOS)
00416                                         return; // non-linear movement reached
00417                                 break;
00418                         case 2:
00419                                 if (GetPVP()->msf == MSF_NLINMOV)
00420                                         return; // linear movement reached
00421                                 break;
00422                 }
00423                 poll_t.WaitUntilElapsed();
00424         }       
00425 }
00426 
00427 void CMotBase::incDegrees(double dif, bool wait, int tolerance, long timeout) {
00428         int dir;
00429         _initialParameters.rotationDirection == DIR_NEGATIVE ? dir = 1 : dir = -1;
00430         int enc = (int) (dif / 360 * dir * (double) _initialParameters.encodersPerCycle);
00431         inc(enc, wait, tolerance, timeout);
00432 }
00433 
00434 void CMotBase::decDegrees(double dif, bool wait, int tolerance, long timeout) {
00435         int dir;
00436         _initialParameters.rotationDirection == DIR_NEGATIVE ? dir = 1 : dir = -1;
00437         int enc = (int) (dif / 360 * dir * (double) _initialParameters.encodersPerCycle);
00438         dec(enc, wait, tolerance, timeout);
00439 }
00440 
00441 void CMotBase::movDegrees(double tar, bool wait, int tolerance, long timeout) {
00442         int enc = KNI_MHF::rad2enc( KNI_MHF::deg2rad(tar), _initialParameters.angleOffset, _initialParameters.encodersPerCycle, _initialParameters.encoderOffset, _initialParameters.rotationDirection);
00443         mov(enc, wait, tolerance, timeout);
00444 }


kni
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:34