$search
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 }