00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 #include <assert.h>
00058 #include <cob_canopen_motor/CanDriveHarmonica.h>
00059 #include <unistd.h>
00060
00061
00062 CanDriveHarmonica::CanDriveHarmonica()
00063 {
00064
00065 m_Param.iDivForRequestStatus = 10;
00066 m_Param.dCanTimeout = 6;
00067
00068
00069 m_pCanCtrl = NULL;
00070
00071 m_iStatusCtrl = 0;
00072 m_dPosGearMeasRad = 0;
00073 m_dAngleGearRadMem = 0;
00074 m_dVelGearMeasRadS = 0;
00075
00076 m_VelCalcTime.SetNow();
00077
00078 m_bLimSwLeft = false;
00079 m_bLimSwRight = false;
00080
00081 m_bLimitSwitchEnabled = false;
00082
00083 m_iCountRequestDiv = 0;
00084
00085 m_iMotorState = ST_PRE_INITIALIZED;
00086 m_bCurrentLimitOn = false;
00087
00088 m_iNumAttempsRecFail = 0;
00089
00090 m_SendTime.SetNow();
00091 m_StartTime.SetNow();
00092
00093 m_bOutputOfFailure = false;
00094
00095 m_bIsInitialized = false;
00096
00097
00098 ElmoRec = new ElmoRecorder(this);
00099
00100 }
00101
00102
00103 void CanDriveHarmonica::setCanOpenParam( int iTxPDO1, int iTxPDO2, int iRxPDO2, int iTxSDO, int iRxSDO )
00104 {
00105 m_ParamCanOpen.iTxPDO1 = iTxPDO1;
00106 m_ParamCanOpen.iTxPDO2 = iTxPDO2;
00107 m_ParamCanOpen.iRxPDO2 = iRxPDO2;
00108 m_ParamCanOpen.iTxSDO = iTxSDO;
00109 m_ParamCanOpen.iRxSDO = iRxSDO;
00110
00111 }
00112
00113
00114 bool CanDriveHarmonica::evalReceivedMsg(CanMsg& msg)
00115 {
00116 bool bRet = false;
00117 int iDigIn;
00118 int iFailure;
00119 int iPara;
00120
00121 int iHomeDigIn = 0x0001;
00122 int iTemp1, iTemp2;
00123
00124 m_CanMsgLast = msg;
00125
00126
00127
00128 if (msg.m_iID == m_ParamCanOpen.iTxPDO1)
00129 {
00130 iTemp1 = (msg.getAt(3) << 24) | (msg.getAt(2) << 16)
00131 | (msg.getAt(1) << 8) | (msg.getAt(0) );
00132
00133 m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam.
00134 PosMotIncrToPosGearRad(iTemp1);
00135
00136 iTemp2 = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00137 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00138
00139 m_dVelGearMeasRadS = m_DriveParam.getSign() * m_DriveParam.
00140 VelMotIncrPeriodToVelGearRadS(iTemp2);
00141
00142 m_WatchdogTime.SetNow();
00143
00144 bRet = true;
00145 }
00146
00147
00148
00149 if (msg.m_iID == m_ParamCanOpen.iTxPDO2)
00150 {
00151 if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'X') )
00152 {
00153 }
00154
00155 else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'A') )
00156 {
00157 }
00158
00159 else if( (msg.getAt(0) == 'J') && (msg.getAt(1) == 'V') )
00160 {
00161 }
00162
00163 else if( (msg.getAt(0) == 'B') && (msg.getAt(1) == 'G') )
00164 {
00165 }
00166
00167 else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') )
00168 {
00169 iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00170 | (msg.getAt(5) << 8) | (msg.getAt(4)) );
00171 }
00172
00173 else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'P') )
00174 {
00175 iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00176 | (msg.getAt(5) << 8) | (msg.getAt(4)) );
00177 iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00178 | (msg.getAt(5) << 8) | (msg.getAt(4)) );
00179
00180 if( (iDigIn & iHomeDigIn) != 0x0000 )
00181 {
00182 m_bLimSwRight = true;
00183 }
00184 }
00185
00186 else if( (msg.getAt(0) == 'S') && (msg.getAt(1) == 'R') )
00187 {
00188 m_iStatusCtrl = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00189 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00190
00191 evalStatusRegister(m_iStatusCtrl);
00192 ElmoRec->readoutRecorderTryStatus(m_iStatusCtrl, seg_Data);
00193
00194 }
00195
00196 else if( (msg.getAt(0) == 'M') && (msg.getAt(1) == 'F') )
00197 {
00198 iFailure = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00199 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00200
00201 evalMotorFailure(iFailure);
00202 }
00203
00204
00205 else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') )
00206 {
00207 iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00208 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00209
00210 std::cout << "um " << iPara << std::endl;
00211 }
00212
00213 else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'M') )
00214 {
00215 iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00216 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00217
00218 std::cout << "pm " << iPara << std::endl;
00219 }
00220
00221 else if( (msg.getAt(0) == 'A') && (msg.getAt(1) == 'C') )
00222 {
00223 iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00224 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00225
00226 std::cout << "ac " << iPara << std::endl;
00227 }
00228
00229 else if( (msg.getAt(0) == 'D') && (msg.getAt(1) == 'C') )
00230 {
00231 iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00232 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00233
00234 std::cout << "dc " << iPara << std::endl;
00235 }
00236 else if( (msg.getAt(0) == 'H') && (msg.getAt(1) == 'M') )
00237 {
00238
00239 if(msg.getAt(4) == 0)
00240 {
00241
00242 m_bLimSwRight = true;
00243 }
00244 }
00245 else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'Q') )
00246 {
00247 int iVal=0;
00248 iVal = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00249 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00250 float* pfVal;
00251 pfVal=(float*)&iVal;
00252 m_dMotorCurr = *pfVal;
00253 }
00254
00255 else
00256 {
00257 }
00258
00259 m_WatchdogTime.SetNow();
00260
00261 bRet = true;
00262 }
00263
00264
00265
00266 if (msg.m_iID == m_ParamCanOpen.iTxSDO)
00267 {
00268 m_WatchdogTime.SetNow();
00269
00270 if( (msg.getAt(0) >> 5) == 0) {
00271
00272 receivedSDODataSegment(msg);
00273
00274 } else if( (msg.getAt(0) & 0xE2) == 0x40) {
00275
00276 receivedSDOSegmentedInitiation(msg);
00277
00278 } else if( (msg.getAt(0) >> 5) == 4) {
00279 unsigned int iErrorNum = (msg.getAt(4) | msg.getAt(5) << 8 | msg.getAt(6) << 16 | msg.getAt(7) << 24);
00280 receivedSDOTransferAbort(iErrorNum);
00281 }
00282
00283 bRet = true;
00284 }
00285
00286 return bRet;
00287 }
00288
00289
00290 bool CanDriveHarmonica::init()
00291 {
00292 int iCnt, iPosCnt;
00293 bool bRet = true;
00294 CanMsg Msg;
00295
00296 m_iMotorState = ST_PRE_INITIALIZED;
00297
00298
00299
00300 int iIncrRevWheel = int( (double)m_DriveParam.getGearRatio() * (double)m_DriveParam.getBeltRatio()
00301 * (double)m_DriveParam.getEncIncrPerRevMot() * 3 );
00302 IntprtSetInt(8, 'M', 'O', 0, 0);
00303 usleep(20000);
00304 IntprtSetInt(8, 'X', 'M', 2, iIncrRevWheel * 5000);
00305 usleep(20000);
00306 IntprtSetInt(8, 'X', 'M', 1, -iIncrRevWheel * 5000);
00307 usleep(20000);
00308
00309
00310 setTypeMotion(MOTIONTYPE_VELCTRL);
00311
00312 IntprtSetInt(8, 'P', 'X', 0, 0);
00313
00314 iCnt = 0;
00315 while(true)
00316 {
00317 m_pCanCtrl->receiveMsg(&Msg);
00318
00319 if( (Msg.getAt(0) == 'P') && (Msg.getAt(1) == 'X') )
00320 {
00321 iPosCnt = (Msg.getAt(7) << 24) | (Msg.getAt(6) << 16)
00322 | (Msg.getAt(5) << 8) | (Msg.getAt(4) );
00323
00324 m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam.PosMotIncrToPosGearRad(iPosCnt);
00325 m_dAngleGearRadMem = m_dPosGearMeasRad;
00326 break;
00327 }
00328
00329 if ( iCnt > 300 )
00330 {
00331 std::cout << "CanDriveHarmonica: initial position not set" << std::endl;
00332 bRet = false;
00333 break;
00334 }
00335
00336 usleep(10000);
00337 iCnt++;
00338 }
00339
00340
00341
00342
00343
00344
00345
00346 sendSDODownload(0x1A00, 0, 0);
00347
00348
00349 sendSDODownload(0x1A00, 1, 0x60640020);
00350
00351
00352 sendSDODownload(0x1A00, 2, 0x60690020);
00353
00354
00355 sendSDODownload(0x1800, 2, 1);
00356
00357
00358 sendSDODownload(0x1A00, 0, 2);
00359
00360 m_bWatchdogActive = false;
00361
00362 if( bRet )
00363 m_bIsInitialized = true;
00364
00365 return bRet;
00366 }
00367
00368 bool CanDriveHarmonica::stop()
00369 {
00370 bool bRet = true;
00371
00372 IntprtSetInt(8, 'M', 'O', 0, 0);
00373 usleep(20000);
00374 return bRet;
00375 }
00376
00377 bool CanDriveHarmonica::start()
00378 {
00379
00380 IntprtSetInt(8, 'M', 'O', 0, 1);
00381 usleep(20000);
00382
00383
00384 int iCnt;
00385 bool bRet = true;
00386 int iStatus;
00387 CanMsg Msg;
00388
00389
00390 do
00391 {
00392 bRet = m_pCanCtrl->receiveMsg(&Msg);
00393 }
00394 while(bRet == true);
00395
00396
00397 IntprtSetInt(4, 'S', 'R', 0, 0);
00398
00399 iCnt = 0;
00400 while(true)
00401 {
00402 m_pCanCtrl->receiveMsg(&Msg);
00403
00404 if( (Msg.getAt(0) == 'S') && (Msg.getAt(1) == 'R') )
00405 {
00406 iStatus = (Msg.getAt(7) << 24) | (Msg.getAt(6) << 16)
00407 | (Msg.getAt(5) << 8) | (Msg.getAt(4) );
00408
00409 bRet = evalStatusRegister(iStatus);
00410 break;
00411 }
00412
00413 if ( iCnt > 300 )
00414 {
00415 std::cout << "CanDriveHarmonica::enableMotor(): No answer on status request" << std::endl;
00416 bRet = false;
00417 break;
00418 }
00419
00420 usleep(10000);
00421 iCnt++;
00422 }
00423
00424
00425 m_WatchdogTime.SetNow();
00426 m_SendTime.SetNow();
00427
00428 return bRet;
00429 }
00430
00431
00432 bool CanDriveHarmonica::reset()
00433 {
00434
00435
00436
00437 CanMsg msg;
00438 msg.m_iID = 0;
00439 msg.m_iLen = 2;
00440 msg.set(1,0,0,0,0,0,0,0);
00441 m_pCanCtrl->transmitMsg(msg);
00442
00443
00444 bool bRet = init();
00445 bRet |= start();
00446
00447 return bRet;
00448 }
00449
00450 bool CanDriveHarmonica::shutdown()
00451 {
00452 std::cout << "shutdown drive " << m_DriveParam.getDriveIdent() << std::endl;
00453
00454 IntprtSetInt(8, 'M', 'O', 0, 0);
00455
00456 return true;
00457 }
00458
00459
00460 bool CanDriveHarmonica::startWatchdog(bool bStarted)
00461 {
00462 if (bStarted == true)
00463 {
00464
00465 m_bWatchdogActive = true;
00466
00467
00468
00469
00470 const int c_iHeartbeatTimeMS = 1000;
00471 const int c_iNMTNodeID = 0x00;
00472
00473
00474 sendSDODownload(0x1016, 1, (c_iNMTNodeID << 16) | c_iHeartbeatTimeMS);
00475
00476
00477 sendSDODownload(0x1029, 1, 2);
00478
00479
00480 sendSDODownload(0x6007, 0, 3);
00481
00482
00483
00484
00485 sendSDODownload(0x2F21, 0, 0x08);
00486 usleep(20000);
00487
00488 }
00489 else
00490 {
00491
00492 m_bWatchdogActive = false;
00493
00494
00495 sendSDODownload(0x6007, 0, 0);
00496
00497
00498 sendSDODownload(0x1029, 1, 1);
00499
00500
00501
00502
00503 sendSDODownload(0x2F21, 0, 0x00);
00504 usleep(25000);
00505
00506
00507 }
00508
00509 return true;
00510 }
00511
00512
00513 bool CanDriveHarmonica::disableBrake(bool bDisabled)
00514 {
00515 return true;
00516 }
00517
00518
00519 double CanDriveHarmonica::getTimeToLastMsg()
00520 {
00521 m_CurrentTime.SetNow();
00522
00523 return m_CurrentTime - m_WatchdogTime;
00524 }
00525
00526 bool CanDriveHarmonica::getStatusLimitSwitch()
00527 {
00528 return m_bLimSwRight;
00529 }
00530
00531 bool CanDriveHarmonica::initHoming()
00532 {
00533 const int c_iPosRef = m_DriveParam.getEncOffset();
00534
00535
00536
00537 IntprtSetInt(8, 'H', 'M', 1, 0);
00538
00539
00540 usleep(20000);
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552 IntprtSetInt(8, 'H', 'M', 2, c_iPosRef);
00553 usleep(20000);
00554
00555
00556
00557
00558
00559
00560 IntprtSetInt(8, 'H', 'M', 3, m_DriveParam.getHomingDigIn());
00561
00562 usleep(20000);
00563
00564
00565
00566
00567
00568 IntprtSetInt(8, 'H', 'M', 4, 2);
00569 usleep(20000);
00570
00571
00572
00573 IntprtSetInt(8, 'H', 'M', 5, 0);
00574 usleep(20000);
00575
00576
00577
00578 return true;
00579 }
00580
00581
00582
00583 bool CanDriveHarmonica::execHoming()
00584 {
00585
00586 int iCnt;
00587 CanMsg Msg;
00588 bool bRet = true;
00589
00590 int iNrDrive = m_DriveParam.getDriveIdent();
00591
00592
00593
00594 IntprtSetInt(8, 'H', 'M', 1, 1);
00595
00596
00597 do
00598 {
00599
00600 bRet = m_pCanCtrl->receiveMsg(&Msg);
00601 }
00602 while(bRet == true);
00603
00604
00605
00606
00607 iCnt = 0;
00608
00609 do
00610 {
00611
00612
00613 IntprtSetInt(4, 'H', 'M', 1, 0);
00614
00615
00616 m_pCanCtrl->receiveMsgRetry(&Msg, 10);
00617
00618
00619 if( (Msg.getAt(0) == 'H') && (Msg.getAt(1) == 'M') )
00620 {
00621
00622 if(Msg.getAt(4) == 0)
00623 {
00624
00625 std::cout << "Got Homing-Signal " << std::endl;
00626 m_bLimSwRight = true;
00627 break;
00628 }
00629 }
00630
00631
00632 usleep(10000);
00633 iCnt++;
00634
00635 }
00636 while((m_bLimSwRight == false) && (iCnt<2000));
00637
00638
00639 if(iCnt>=2000)
00640 {
00641 std::cout << "Homing failed - limit switch " << iNrDrive << " not reached" << std::endl;
00642 bRet = false;
00643 }
00644 else
00645 {
00646 std::cout << "Homing successful - limit switch " << iNrDrive << " ok" << std::endl;
00647 bRet = true;
00648 }
00649
00650
00651
00652 return bRet;
00653 }
00654
00655 void CanDriveHarmonica::setGearPosVelRadS(double dPosGearRad, double dVelGearRadS)
00656 {
00657 int iPosEncIncr;
00658 int iVelEncIncrPeriod;
00659
00660 m_DriveParam.PosVelRadToIncr(dPosGearRad, dVelGearRadS, &iPosEncIncr, &iVelEncIncrPeriod);
00661
00662 if(iVelEncIncrPeriod > m_DriveParam.getVelMax())
00663 {
00664 iVelEncIncrPeriod = (int)m_DriveParam.getVelMax();
00665 }
00666
00667 if(iVelEncIncrPeriod < -m_DriveParam.getVelMax())
00668 {
00669 iVelEncIncrPeriod = (int)-m_DriveParam.getVelMax();
00670 }
00671
00672 if(m_iTypeMotion == MOTIONTYPE_POSCTRL)
00673 {
00674
00675 IntprtSetInt(8, 'S', 'P', 0, iVelEncIncrPeriod);
00676
00677
00678
00679
00680 if (m_DriveParam.getIsSteer() == true)
00681 IntprtSetInt(8, 'P', 'A', 0, iPosEncIncr);
00682 else
00683 IntprtSetInt(8, 'P', 'R', 0, iPosEncIncr);
00684
00685 IntprtSetInt(4, 'B', 'G', 0, 0);
00686
00687 }
00688
00689 if(m_iTypeMotion == MOTIONTYPE_VELCTRL)
00690 {
00691 iVelEncIncrPeriod *= m_DriveParam.getSign();
00692 IntprtSetInt(8, 'J', 'V', 0, iVelEncIncrPeriod);
00693 IntprtSetInt(4, 'B', 'G', 0, 0);
00694 }
00695
00696
00697
00698 CanMsg msg;
00699 msg.m_iID = 0x80;
00700 msg.m_iLen = 0;
00701 msg.set(0,0,0,0,0,0,0,0);
00702 m_pCanCtrl->transmitMsg(msg);
00703 }
00704
00705
00706 void CanDriveHarmonica::setGearVelRadS(double dVelGearRadS)
00707 {
00708 int iVelEncIncrPeriod;
00709
00710
00711 iVelEncIncrPeriod = m_DriveParam.getSign() * m_DriveParam.VelGearRadSToVelMotIncrPeriod(dVelGearRadS);
00712
00713 if(iVelEncIncrPeriod > m_DriveParam.getVelMax())
00714 {
00715 std::cout << "SteerVelo asked for " << iVelEncIncrPeriod << " EncIncrements" << std::endl;
00716 iVelEncIncrPeriod = (int)m_DriveParam.getVelMax();
00717 }
00718
00719 if(iVelEncIncrPeriod < -m_DriveParam.getVelMax())
00720 {
00721 std::cout << "SteerVelo asked for " << iVelEncIncrPeriod << " EncIncrements" << std::endl;
00722 iVelEncIncrPeriod = -1 * (int)m_DriveParam.getVelMax();
00723 }
00724
00725 IntprtSetInt(8, 'J', 'V', 0, iVelEncIncrPeriod);
00726 IntprtSetInt(4, 'B', 'G', 0, 0);
00727
00728
00729
00730
00731 CanMsg msg;
00732 msg.m_iID = 0x80;
00733 msg.m_iLen = 0;
00734 msg.set(0,0,0,0,0,0,0,0);
00735 m_pCanCtrl->transmitMsg(msg);
00736
00737
00738 msg.m_iID = 0x700;
00739 msg.m_iLen = 5;
00740 msg.set(0x00,0,0,0,0,0,0,0);
00741 m_pCanCtrl->transmitMsg(msg);
00742
00743 m_CurrentTime.SetNow();
00744 double dt = m_CurrentTime - m_SendTime;
00745 if ((dt > 1.0) && m_bWatchdogActive)
00746 {
00747 std::cout << "Time between send velocity of motor " << m_DriveParam.getDriveIdent()
00748 << " is too large: " << dt << " s" << std::endl;
00749 }
00750 m_SendTime.SetNow();
00751
00752
00753
00754 m_iCountRequestDiv++;
00755 if (m_iCountRequestDiv > m_Param.iDivForRequestStatus)
00756 {
00757 requestStatus();
00758 m_iCountRequestDiv = 0;
00759 }
00760 }
00761
00762
00763 void CanDriveHarmonica::getGearPosRad(double* dGearPosRad)
00764 {
00765 *dGearPosRad = m_dPosGearMeasRad;
00766 }
00767
00768
00769 void CanDriveHarmonica::getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS)
00770 {
00771 *pdAngleGearRad = m_dPosGearMeasRad;
00772 *pdVelGearRadS = m_dVelGearMeasRadS;
00773 }
00774
00775
00776 void CanDriveHarmonica::getGearDeltaPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS)
00777 {
00778 *pdAngleGearRad = m_dPosGearMeasRad - m_dAngleGearRadMem;
00779 *pdVelGearRadS = m_dVelGearMeasRadS;
00780 m_dAngleGearRadMem = m_dPosGearMeasRad;
00781 }
00782
00783
00784 void CanDriveHarmonica::getData(double* pdPosGearRad, double* pdVelGearRadS,
00785 int* piTorqueCtrl, int* piStatusCtrl)
00786 {
00787 *pdPosGearRad = m_dPosGearMeasRad;
00788 *pdVelGearRadS = m_dVelGearMeasRadS;
00789 *piTorqueCtrl = m_iTorqueCtrl;
00790 *piStatusCtrl = m_iStatusCtrl;
00791 }
00792
00793
00794 void CanDriveHarmonica::requestPosVel()
00795 {
00796
00797 CanMsg msg;
00798 msg.m_iID = 0x80;
00799 msg.m_iLen = 0;
00800 msg.set(0,0,0,0,0,0,0,0);
00801 m_pCanCtrl->transmitMsg(msg);
00802
00803 }
00804
00805
00806 void CanDriveHarmonica::sendHeartbeat()
00807 {
00808 CanMsg msg;
00809 msg.m_iID = 0x700;
00810 msg.m_iLen = 5;
00811 msg.set(0x00,0,0,0,0,0,0,0);
00812 m_pCanCtrl->transmitMsg(msg);
00813 }
00814
00815
00816 void CanDriveHarmonica::requestStatus()
00817 {
00818 IntprtSetInt(4, 'S', 'R', 0, 0);
00819 }
00820
00821
00822 void CanDriveHarmonica::requestMotorTorque()
00823 {
00824
00825 IntprtSetInt(4, 'I', 'Q', 0, 0);
00826
00827 }
00828
00829
00830 bool CanDriveHarmonica::isError()
00831 {
00832 if (m_iMotorState != ST_MOTOR_FAILURE)
00833 {
00834
00835 double dWatchTime = getTimeToLastMsg();
00836
00837 if (dWatchTime>m_Param.dCanTimeout)
00838 {
00839 if ( m_bOutputOfFailure == false)
00840 {
00841 std::cout << "Motor " << m_DriveParam.getDriveIdent() <<
00842 " has no can communication for " << dWatchTime << " s." << std::endl;
00843 }
00844
00845 m_iMotorState = ST_MOTOR_FAILURE;
00846 m_FailureStartTime.SetNow();
00847 }
00848
00849 }
00850
00851 return (m_iMotorState == ST_MOTOR_FAILURE);
00852 }
00853
00854 bool CanDriveHarmonica::setTypeMotion(int iType)
00855 {
00856 int iMaxAcc = int(m_DriveParam.getMaxAcc());
00857 int iMaxDcc = int(m_DriveParam.getMaxDec());
00858 CanMsg Msg;
00859
00860 if (iType == MOTIONTYPE_POSCTRL)
00861 {
00862
00863
00864
00865 IntprtSetInt(8, 'M', 'O', 0, 0);
00866 usleep(20000);
00867
00868 IntprtSetInt(8, 'U', 'M', 0, 5);
00869
00870
00871 IntprtSetInt(8, 'T', 'R', 1, 15);
00872
00873 IntprtSetInt(8, 'T', 'R', 2, 100);
00874
00875
00876 IntprtSetInt(8, 'A', 'C', 0, iMaxAcc);
00877
00878 IntprtSetInt(8, 'D', 'C', 0, iMaxDcc);
00879 usleep(100000);
00880
00881
00882 }
00883 else if (iType == MOTIONTYPE_TORQUECTRL)
00884 {
00885
00886
00887 IntprtSetInt(8, 'M', 'O', 0, 0);
00888 usleep(50000);
00889
00890 IntprtSetInt(8, 'U', 'M', 0, 1);
00891
00892
00893 IntprtSetInt(8, 'R', 'M', 0, 0);
00894
00895
00896 std::cout << "Motor"<<m_DriveParam.getDriveIdent()<<" Unit Mode switched to: TORQUE controlled" << std::endl;
00897 usleep(100000);
00898 }
00899 else
00900 {
00901
00902
00903 IntprtSetInt(8, 'M', 'O', 0, 0);
00904
00905 IntprtSetInt(8, 'U', 'M', 0, 2);
00906
00907 IntprtSetInt(8, 'P', 'M', 0, 1);
00908
00909
00910 IntprtSetInt(8, 'A', 'C', 0, iMaxAcc);
00911
00912 IntprtSetInt(8, 'D', 'C', 0, iMaxDcc);
00913 usleep(100000);
00914 }
00915
00916 m_iTypeMotion = iType;
00917 return true;
00918 }
00919
00920
00921
00922 void CanDriveHarmonica::IntprtSetInt(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData)
00923 {
00924 char cIndex[2];
00925 char cInt[4];
00926 CanMsg CMsgTr;
00927
00928 CMsgTr.m_iID = m_ParamCanOpen.iRxPDO2;
00929 CMsgTr.m_iLen = iDataLen;
00930
00931 cIndex[0] = iIndex;
00932 cIndex[1] = (iIndex >> 8) & 0x3F;
00933
00934 cInt[0] = iData;
00935 cInt[1] = iData >> 8;
00936 cInt[2] = iData >> 16;
00937 cInt[3] = iData >> 24;
00938
00939 CMsgTr.set(cCmdChar1, cCmdChar2, cIndex[0], cIndex[1], cInt[0], cInt[1], cInt[2], cInt[3]);
00940 m_pCanCtrl->transmitMsg(CMsgTr);
00941 }
00942
00943
00944 void CanDriveHarmonica::IntprtSetFloat(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, float fData)
00945 {
00946 char cIndex[2];
00947 char cFloat[4];
00948 CanMsg CMsgTr;
00949 char* pTempFloat = NULL;
00950
00951 CMsgTr.m_iID = m_ParamCanOpen.iRxPDO2;
00952 CMsgTr.m_iLen = iDataLen;
00953
00954 cIndex[0] = iIndex;
00955
00956 cIndex[1] = (iIndex >> 8) & 0x3F;
00957 cIndex[1] = cIndex[1] | 0x80;
00958
00959 pTempFloat = (char*)&fData;
00960 for( int i=0; i<4; i++ )
00961 cFloat[i] = pTempFloat[i];
00962
00963 CMsgTr.set(cCmdChar1, cCmdChar2, cIndex[0], cIndex[1], cFloat[0], cFloat[1], cFloat[2], cFloat[3]);
00964 m_pCanCtrl->transmitMsg(CMsgTr);
00965 }
00966
00967
00968
00969
00970
00971
00972 void CanDriveHarmonica::sendSDOAbort(int iObjIndex, int iObjSubIndex, unsigned int iErrorCode)
00973 {
00974 CanMsg CMsgTr;
00975 const int ciAbortTransferReq = 0x04 << 5;
00976
00977 CMsgTr.m_iLen = 8;
00978 CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
00979
00980 unsigned char cMsg[8];
00981
00982 cMsg[0] = ciAbortTransferReq;
00983 cMsg[1] = iObjIndex;
00984 cMsg[2] = iObjIndex >> 8;
00985 cMsg[3] = iObjSubIndex;
00986 cMsg[4] = iErrorCode;
00987 cMsg[5] = iErrorCode >> 8;
00988 cMsg[6] = iErrorCode >> 16;
00989 cMsg[7] = iErrorCode >> 24;
00990
00991 CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
00992 m_pCanCtrl->transmitMsg(CMsgTr);
00993 }
00994
00995
00996 void CanDriveHarmonica::receivedSDOTransferAbort(unsigned int iErrorCode){
00997 std::cout << "SDO Abort Transfer received with error code: " << iErrorCode;
00998 seg_Data.statusFlag = segData::SDO_SEG_FREE;
00999 }
01000
01001
01002 void CanDriveHarmonica::sendSDOUpload(int iObjIndex, int iObjSubIndex)
01003 {
01004 CanMsg CMsgTr;
01005 const int ciInitUploadReq = 0x40;
01006
01007 CMsgTr.m_iLen = 8;
01008 CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
01009
01010 unsigned char cMsg[8];
01011
01012 cMsg[0] = ciInitUploadReq;
01013 cMsg[1] = iObjIndex;
01014 cMsg[2] = iObjIndex >> 8;
01015 cMsg[3] = iObjSubIndex;
01016 cMsg[4] = 0x00;
01017 cMsg[5] = 0x00;
01018 cMsg[6] = 0x00;
01019 cMsg[7] = 0x00;
01020
01021 CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
01022 m_pCanCtrl->transmitMsg(CMsgTr);
01023 }
01024
01025
01026 void CanDriveHarmonica::sendSDODownload(int iObjIndex, int iObjSubIndex, int iData)
01027 {
01028 CanMsg CMsgTr;
01029
01030 const int ciInitDownloadReq = 0x20;
01031 const int ciNrBytesNoData = 0x00;
01032 const int ciExpedited = 0x02;
01033 const int ciDataSizeInd = 0x01;
01034
01035 CMsgTr.m_iLen = 8;
01036 CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
01037
01038 unsigned char cMsg[8];
01039
01040 cMsg[0] = ciInitDownloadReq | (ciNrBytesNoData << 2) | ciExpedited | ciDataSizeInd;
01041 cMsg[1] = iObjIndex;
01042 cMsg[2] = iObjIndex >> 8;
01043 cMsg[3] = iObjSubIndex;
01044 cMsg[4] = iData;
01045 cMsg[5] = iData >> 8;
01046 cMsg[6] = iData >> 16;
01047 cMsg[7] = iData >> 24;
01048
01049 CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
01050 m_pCanCtrl->transmitMsg(CMsgTr);
01051 }
01052
01053
01054 void CanDriveHarmonica::evalSDO(CanMsg& CMsg, int* pIndex, int* pSubindex)
01055 {
01056 *pIndex = (CMsg.getAt(2) << 8) | CMsg.getAt(1);
01057 *pSubindex = CMsg.getAt(3);
01058 }
01059
01060
01061 int CanDriveHarmonica::getSDODataInt32(CanMsg& CMsg)
01062 {
01063 int iData = (CMsg.getAt(7) << 24) | (CMsg.getAt(6) << 16) |
01064 (CMsg.getAt(5) << 8) | CMsg.getAt(4);
01065
01066 return iData;
01067 }
01068
01069
01070 int CanDriveHarmonica::receivedSDOSegmentedInitiation(CanMsg& msg) {
01071
01072 if(seg_Data.statusFlag == segData::SDO_SEG_FREE || seg_Data.statusFlag == segData::SDO_SEG_WAITING) {
01073 seg_Data.resetTransferData();
01074 seg_Data.statusFlag = segData::SDO_SEG_COLLECTING;
01075
01076
01077 evalSDO(msg, &seg_Data.objectID, &seg_Data.objectSubID);
01078
01079
01080 if( (msg.getAt(0) & 0x01) == 1) {
01081 seg_Data.numTotalBytes = msg.getAt(7) << 24 | msg.getAt(6) << 16 | msg.getAt(5) << 8 | msg.getAt(4);
01082 } else seg_Data.numTotalBytes = 0;
01083
01084 sendSDOUploadSegmentConfirmation(seg_Data.toggleBit);
01085 }
01086
01087 return 0;
01088
01089 }
01090
01091
01092 int CanDriveHarmonica::receivedSDODataSegment(CanMsg& msg){
01093
01094 int numEmptyBytes = 0;
01095
01096
01097
01098
01099
01100 if( (msg.getAt(0) & 0x10) != (seg_Data.toggleBit << 4) ) {
01101 std::cout << "Toggle Bit error, send Abort SDO with \"Toggle bit not alternated\" error" << std::endl;
01102 sendSDOAbort(seg_Data.objectID, seg_Data.objectSubID, 0x05030000);
01103 return 1;
01104 }
01105
01106 if( (msg.getAt(0) & 0x01) == 0x00) {
01107 seg_Data.statusFlag = segData::SDO_SEG_COLLECTING;
01108 } else {
01109
01110 seg_Data.statusFlag = segData::SDO_SEG_PROCESSING;
01111 };
01112
01113 numEmptyBytes = (msg.getAt(0) >> 1) & 0x07;
01114
01115
01116 for(int i=1; i<=7-numEmptyBytes; i++) {
01117 seg_Data.data.push_back(msg.getAt(i));
01118 }
01119
01120 if(seg_Data.statusFlag == segData::SDO_SEG_PROCESSING) {
01121 finishedSDOSegmentedTransfer();
01122 } else {
01123 seg_Data.toggleBit = !seg_Data.toggleBit;
01124 sendSDOUploadSegmentConfirmation(seg_Data.toggleBit);
01125 }
01126
01127 return 0;
01128 }
01129
01130
01131 void CanDriveHarmonica::sendSDOUploadSegmentConfirmation(bool toggleBit) {
01132
01133 CanMsg CMsgTr;
01134 int iConfirmSegment = 0x60;
01135 iConfirmSegment = iConfirmSegment | (toggleBit << 4);
01136
01137 CMsgTr.m_iLen = 8;
01138 CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
01139
01140 unsigned char cMsg[8];
01141
01142 cMsg[0] = iConfirmSegment;
01143 cMsg[1] = 0x00;
01144 cMsg[2] = 0x00;
01145 cMsg[3] = 0x00;
01146 cMsg[4] = 0x00;
01147 cMsg[5] = 0x00;
01148 cMsg[6] = 0x00;
01149 cMsg[7] = 0x00;
01150
01151 CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
01152 m_pCanCtrl->transmitMsg(CMsgTr);
01153 }
01154
01155
01156 void CanDriveHarmonica::finishedSDOSegmentedTransfer() {
01157 seg_Data.statusFlag = segData::SDO_SEG_PROCESSING;
01158
01159 if( (seg_Data.data.size() != seg_Data.numTotalBytes) & (seg_Data.numTotalBytes != 0) ) {
01160 std::cout << "WARNING: SDO tranfer finished but number of collected bytes "
01161 << seg_Data.data.size() << " != expected number of bytes: " << seg_Data.numTotalBytes << std::endl;
01162
01163 }
01164
01165 if(seg_Data.objectID == 0x2030) {
01166 if(ElmoRec->processData(seg_Data) == 0) seg_Data.statusFlag = segData::SDO_SEG_FREE;
01167 }
01168 }
01169
01170
01171 double CanDriveHarmonica::estimVel(double dPos)
01172 {
01173 double dVel;
01174 double dt;
01175
01176 m_CurrentTime.SetNow();
01177
01178 dt = m_CurrentTime - m_VelCalcTime;
01179
01180 dVel = (dPos - m_dOldPos)/dt;
01181
01182 m_dOldPos = dPos;
01183 m_VelCalcTime.SetNow();
01184
01185 return dVel;
01186 }
01187
01188 bool CanDriveHarmonica::evalStatusRegister(int iStatus)
01189 {
01190 bool bNoError;
01191
01192
01193 if( isBitSet(iStatus, 0) )
01194 {
01195
01196 if ( m_bOutputOfFailure == false )
01197 {
01198 std::cout << "Error of drive: " << m_DriveParam.getDriveIdent() << std::endl;
01199
01200 if( (iStatus & 0x0000000E) == 2)
01201 std::cout << "- drive error under voltage" << std::endl;
01202
01203 if( (iStatus & 0x0000000E) == 4)
01204 std::cout << "- drive error over voltage" << std::endl;
01205
01206 if( (iStatus & 0x0000000E) == 10)
01207 std::cout << "- drive error short circuit" << std::endl;
01208
01209 if( (iStatus & 0x0000000E) == 12)
01210 std::cout << "- drive error overheating" << std::endl;
01211
01212
01213 IntprtSetInt(4, 'M', 'F', 0, 0);
01214 }
01215
01216 m_iNewMotorState = ST_MOTOR_FAILURE;
01217
01218 bNoError = false;
01219 }
01220 else if ( isBitSet(iStatus, 6) )
01221 {
01222
01223 if ( m_bOutputOfFailure == false )
01224 {
01225 std::cout << "Motor " << m_DriveParam.getDriveIdent() << " failure latched" << std::endl;
01226
01227
01228 IntprtSetInt(4, 'M', 'F', 0, 0);
01229
01230 m_FailureStartTime.SetNow();
01231 }
01232 m_iNewMotorState = ST_MOTOR_FAILURE;
01233
01234 bNoError = false;
01235 }
01236 else
01237 {
01238
01239 bNoError = true;
01240
01241
01242
01243
01244 m_bOutputOfFailure = false;
01245
01246
01247
01248 if( isBitSet(iStatus, 4) )
01249 {
01250 if (m_iMotorState != ST_OPERATION_ENABLED)
01251 {
01252 std::cout << "Motor " << m_DriveParam.getDriveIdent() << " operation enabled" << std::endl;
01253 m_FailureStartTime.SetNow();
01254 }
01255
01256 m_iNewMotorState = ST_OPERATION_ENABLED;
01257 }
01258 else
01259 {
01260 if (m_iMotorState != ST_OPERATION_DISABLED)
01261 {
01262 std::cout << "Motor " << m_DriveParam.getDriveIdent() << " operation disabled" << std::endl;
01263 }
01264
01265 m_iNewMotorState = ST_OPERATION_DISABLED;
01266 }
01267
01268
01269 if( isBitSet(iStatus, 13) )
01270 {
01271 if (m_bCurrentLimitOn == false)
01272 std::cout << "Motor " << m_DriveParam.getDriveIdent() << "current limit on" << std::endl;
01273
01274 m_bCurrentLimitOn = true;
01275 }
01276 else
01277 m_bCurrentLimitOn = false;
01278 }
01279
01280
01281 m_iMotorState = m_iNewMotorState;
01282
01283 if (m_iMotorState == ST_MOTOR_FAILURE)
01284 m_bOutputOfFailure = true;
01285
01286 return bNoError;
01287 }
01288
01289
01290 void CanDriveHarmonica::evalMotorFailure(int iFailure)
01291 {
01292
01293 std::cout << "Motor " << m_DriveParam.getDriveIdent() << " has a failure: " << iFailure << std::endl;
01294
01295 if( isBitSet(iFailure, 2) )
01296 {
01297 std::cout << "- feedback loss" << std::endl;
01298 }
01299
01300 if( isBitSet(iFailure, 3) )
01301 {
01302 std::cout << "- peak current excced" << std::endl;
01303 }
01304
01305 if( isBitSet(iFailure, 7) )
01306 {
01307 std::cout << "- speed track error" << std::endl;
01308 }
01309
01310 if( isBitSet(iFailure, 8) )
01311 {
01312 std::cout << "- position track error" << std::endl;
01313 }
01314
01315 if( isBitSet(iFailure, 17) )
01316 {
01317 std::cout << "- speed limit exceeded" << std::endl;
01318 }
01319
01320 if( isBitSet(iFailure, 21) )
01321 {
01322 std::cout << "- motor stuck" << std::endl;
01323 }
01324 }
01325
01326
01327 void CanDriveHarmonica::setMotorTorque(double dTorqueNm)
01328 {
01329
01330 float fMotCurr = m_DriveParam.getSign() * dTorqueNm / m_DriveParam.getCurrToTorque();
01331
01332
01333 if (fMotCurr > m_DriveParam.getCurrMax())
01334 {
01335 fMotCurr = m_DriveParam.getCurrMax();
01336 std::cout << "Torque command too high: " << fMotCurr << " Nm. Torque has been limitited." << std::endl;
01337 }
01338 if (fMotCurr < -m_DriveParam.getCurrMax())
01339 {
01340 fMotCurr = -m_DriveParam.getCurrMax();
01341 std::cout << "Torque command too high: " << fMotCurr << " Nm. Torque has been limitited." << std::endl;
01342 }
01343
01344
01345 IntprtSetFloat(8, 'T', 'C', 0, fMotCurr);
01346
01347
01348 CanMsg msg;
01349 msg.m_iID = 0x80;
01350 msg.m_iLen = 0;
01351 msg.set(0,0,0,0,0,0,0,0);
01352 m_pCanCtrl->transmitMsg(msg);
01353
01354
01355 sendHeartbeat();
01356
01357 m_CurrentTime.SetNow();
01358 double dt = m_CurrentTime - m_SendTime;
01359 if (dt > 1.0)
01360 {
01361 std::cout << "Time between send current/torque of motor " << m_DriveParam.getDriveIdent()
01362 << " is too large: " << dt << " s" << std::endl;
01363 }
01364 m_SendTime.SetNow();
01365
01366
01367
01368 m_iCountRequestDiv++;
01369 if (m_iCountRequestDiv > m_Param.iDivForRequestStatus)
01370 {
01371 requestStatus();
01372 m_iCountRequestDiv = 0;
01373 }
01374
01375 }
01376
01377
01378 void CanDriveHarmonica::getMotorTorque(double* dTorqueNm)
01379 {
01380
01381 *dTorqueNm = m_DriveParam.getSign() * m_dMotorCurr * m_DriveParam.getCurrToTorque();
01382
01383 }
01384
01385
01386
01387
01388
01389
01390
01391
01392 int CanDriveHarmonica::setRecorder(int iFlag, int iParam, std::string sParam) {
01393
01394 switch(iFlag) {
01395 case 0:
01396 if(iParam < 1) iParam = 1;
01397 ElmoRec->isInitialized(true);
01398 ElmoRec->configureElmoRecorder(iParam, m_DriveParam.getDriveIdent());
01399 return 0;
01400
01401 case 1:
01402 if(!ElmoRec->isInitialized(false)) return 1;
01403
01404 if(seg_Data.statusFlag == segData::SDO_SEG_FREE) {
01405 if( (iParam != 1) && (iParam != 2) && (iParam != 10) && (iParam != 16) ) {
01406 iParam = 1;
01407 std::cout << "Changed the Readout object to #1 as your selected object hasn't been recorded!" << std::endl;
01408 }
01409 ElmoRec->setLogFilename(sParam);
01410 seg_Data.statusFlag = segData::SDO_SEG_WAITING;
01411 ElmoRec->readoutRecorderTry(iParam);
01412 return 0;
01413 } else {
01414 std::cout << "Previous transmission not finished or colected data hasn't been proceeded yet" << std::endl;
01415 return 2;
01416 }
01417
01418 break;
01419
01420 case 2:
01421 if(seg_Data.statusFlag == segData::SDO_SEG_COLLECTING) {
01422
01423 return 2;
01424 } else if(seg_Data.statusFlag == segData::SDO_SEG_PROCESSING) {
01425
01426 return 2;
01427 } else if(seg_Data.statusFlag == segData::SDO_SEG_WAITING) {
01428
01429 return 2;
01430 } else {
01431 return 0;
01432 }
01433
01434 break;
01435
01436 case 99:
01437 sendSDOAbort(0x2030, 0x00, 0x08000020);
01438 seg_Data.resetTransferData();
01439 return 0;
01440 }
01441
01442 return 0;
01443 }