DS402Node.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK Canopen Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 // -- END LICENSE BLOCK ------------------------------------------------
00014 
00015 //----------------------------------------------------------------------
00023 //----------------------------------------------------------------------
00024 
00025 #include "DS402Node.h"
00026 
00027 #include "exceptions.h"
00028 #include "sync.h"
00029 
00030 
00031 
00032 namespace icl_hardware {
00033 namespace canopen_schunk {
00034 
00035 DS402Node::DS402Node(const uint8_t node_id, const CanDevPtr& can_device, HeartBeatMonitor::Ptr heartbeat_monitor):
00036   DS301Node(node_id,can_device, heartbeat_monitor),
00037   m_interpolation_cycle_time_ms(20),
00038   m_max_number_of_state_transitions(10),
00039   m_homing_method(0),
00040   m_transmission_factor(1)
00041 {
00042 }
00043 
00044 void DS402Node::setDefaultPDOMapping (const DS402Node::eDefaultPDOMapping mapping)
00045 {
00046   PDO::MappingConfigurationList rpdo_mappings;
00047   PDO::MappingConfigurationList tpdo_mappings;
00048   switch (mapping)
00049   {
00050     case PDO_MAPPING_CONTROLWORD_STATUSWORD:
00051     {
00052       // Map control and status word to the first PDO (receive and transmit respectively)
00053       rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00054       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6041, 0, 16, "status_word"));
00055       initPDOMappingSingle (rpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00056       initPDOMappingSingle (tpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::TRANSMIT_PDO);
00057       break;
00058     }
00059     case PDO_MAPPING_INTERPOLATED_POSITION_MODE:
00060     {
00061       // Map control and status word to the first PDO (receive and transmit respectively)
00062       rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00063       rpdo_mappings.push_back(PDO::MappingConfiguration(0x60C1, 1, 32, "interpolation_buffer"));
00064 
00065       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6041, 0, 16, "status_word"));
00066       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6064, 0, 32, "measured_position"));
00067 
00068       // Schunk powerballs have already mapped default PDOs. So we use a dummy mapping
00069       initPDOMappingSingle (rpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00070       initPDOMappingSingle (tpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::TRANSMIT_PDO);
00071       break;
00072     }
00073     case PDO_MAPPING_PROFILE_POSITION_MODE:
00074     {
00075       // Map control and status word to the first PDO (receive and transmit respectively)
00076       rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00077       rpdo_mappings.push_back(PDO::MappingConfiguration(0x607A, 0, 32, "target_position"));
00078 
00079       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6041, 0, 16, "status_word"));
00080       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6064, 0, 32, "measured_position"));
00081 
00082       // Schunk powerballs have already mapped default PDOs. So we use a dummy mapping
00083       initPDOMappingSingle (rpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00084       initPDOMappingSingle (tpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::TRANSMIT_PDO);
00085       break;
00086     }
00087     default:
00088     {
00089       break;
00090     }
00091   }
00092 
00093 }
00094 
00095 
00096 bool DS402Node::setTarget(const float target)
00097 {
00098   int64_t target_ticks = m_transmission_factor * target;
00099   bool success = false;
00100   switch (m_op_mode)
00101   {
00102     case ds402::MOO_PROFILE_POSITION_MODE:
00103     {
00104       success = setRPDOValue("target_position", static_cast<int32_t>(target_ticks));
00105       break;
00106     }
00107     case ds402::MOO_VELOCITY_MODE:
00108     {
00109       success = setRPDOValue("vl_target_velocity", static_cast<int16_t>(target_ticks)); //6042
00110       break;
00111     }
00112     case ds402::MOO_PROFILE_VELOCITY_MODE:
00113     {
00114       success = setRPDOValue("target_velocity", static_cast<int32_t>(target_ticks)); //60FF
00115       break;
00116     }
00117     case ds402::MOO_PROFILE_TORQUE_MODE:
00118     {
00119       success = setRPDOValue("target_torque", static_cast<int16_t>(target_ticks)); //6071
00120       break;
00121     }
00122     case ds402::MOO_HOMING_MODE:
00123     {
00124       LOGGING_ERROR_C (CanOpen, DS402Node, "Homing mode does not know a target value." << endl);
00125       success = false;
00126       break;
00127     }
00128     case ds402::MOO_INTERPOLATED_POSITION_MODE:
00129     {
00130       success = setRPDOValue("interpolation_buffer", static_cast<int32_t>(target_ticks));
00131       break;
00132     }
00133     case ds402::MOO_CYCLIC_SYNC_POSITION_MODE:
00134     {
00135       LOGGING_WARNING_C (CanOpen, DS402Node, "Target for cyclic sync position mode is not yet supported." << endl);
00136       success = false;
00137       break;
00138     }
00139     case ds402::MOO_CYCLIC_SYNC_VELOCITY_MODE:
00140     {
00141       LOGGING_WARNING_C (CanOpen, DS402Node, "Target for cyclic sync velocity mode is not yet supported." << endl);
00142       success = false;
00143       break;
00144     }
00145     case ds402::MOO_CYCLIC_SYNC_TORQUE_MODE:
00146     {
00147       LOGGING_WARNING_C (CanOpen, DS402Node, "Target for cyclic sync torque mode is not yet supported." << endl);
00148       success = false;
00149       break;
00150     }
00151     default:
00152     {
00153       LOGGING_ERROR_C (CanOpen, DS402Node, "No legal mode of operation is set. setTarget() is non-functional. " << endl);
00154       break;
00155     }
00156   }
00157   if (success)
00158   {
00159     LOGGING_DEBUG_C(CanOpen, DS402Node, "Set target " << target << " for node " << m_node_id << endl);
00160 
00161   }
00162   return success;
00163 }
00164 
00165 void DS402Node::startPPMovement()
00166 {
00167   ds402::Controlword word;
00168   word.all = getRPDOValue<uint16_t> ("control_word");
00169   word.bit.operation_mode_specific_0 = 1;
00170   setRPDOValue ("control_word", word.all);
00171   LOGGING_DEBUG_C(CanOpen, DS402Node, "Triggered motion for node " << m_node_id << endl);
00172 
00173 }
00174 
00175 void DS402Node::acceptPPTargets ()
00176 {
00177   ds402::Controlword word;
00178   word.all = getRPDOValue<uint16_t> ("control_word");
00179   word.bit.operation_mode_specific_0 = 0;
00180   setRPDOValue ("control_word", word.all);
00181   LOGGING_DEBUG_C(CanOpen, DS402Node, "Accepting new targets for node " << m_node_id << endl);
00182 
00183 }
00184 
00185 double DS402Node::getTargetFeedback()
00186 {
00187   int32_t feedback;
00188   switch (m_op_mode)
00189   {
00190     case ds402::MOO_PROFILE_POSITION_MODE:
00191     {
00192       feedback = getTPDOValue<int32_t>("measured_position");
00193       break;
00194     }
00195     case ds402::MOO_VELOCITY_MODE:
00196     {
00197       // TODO: implement me
00198       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for velocity mode." << endl);
00199       return 0;
00200     }
00201     case ds402::MOO_PROFILE_VELOCITY_MODE:
00202     {
00203       // TODO: implement me
00204       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for profile velocity mode." << endl);
00205       return 0;
00206     }
00207     case ds402::MOO_PROFILE_TORQUE_MODE:
00208     {
00209       // TODO: implement me
00210       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for profile torque mode." << endl);
00211       return 0;
00212     }
00213     case ds402::MOO_HOMING_MODE:
00214     {
00215       // TODO: implement me
00216 //       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for homing mode." << endl);
00217       return 0;
00218     }
00219     case ds402::MOO_INTERPOLATED_POSITION_MODE:
00220     {
00221       feedback = getTPDOValue<int32_t>("measured_position");
00222       break;
00223     }
00224     case ds402::MOO_CYCLIC_SYNC_POSITION_MODE:
00225     {
00226       // TODO: implement me
00227       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for cyclic sync position mode." << endl);
00228       return 0;
00229     }
00230     case ds402::MOO_CYCLIC_SYNC_VELOCITY_MODE:
00231     {
00232       // TODO: implement me
00233       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for cyclic sync velocity mode." << endl);
00234       return 0;
00235     }
00236     case ds402::MOO_CYCLIC_SYNC_TORQUE_MODE:
00237     {
00238       // TODO: implement me
00239       LOGGING_ERROR (CanOpen, "GetTargetFeature is not yet implemented for cyclic sync torque mode." << endl);
00240       return 0;
00241     }
00242     default:
00243     {
00244       LOGGING_ERROR_C (CanOpen, DS402Node, "No legal mode of operation is set. getTargetFeedback() is non-functional. " << endl);
00245       return 0;
00246     }
00247   }
00248 
00249   #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00250   if (m_ws_broadcaster)
00251   {
00252     m_ws_broadcaster->robot->setJointPosition(feedback, m_node_id);
00253   }
00254   #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00255 
00256 
00257   return static_cast<double>(feedback) / m_transmission_factor;
00258 }
00259 
00260 
00261 void DS402Node::setNMTState(const NMT::eNMT_State state, const NMT::eNMT_SubState sub_state)
00262 {
00263   // TODO: implement me!
00264 }
00265 
00266 void DS402Node::quickStop()
00267 {
00268   if (m_current_ds402_state == ds402::STATE_OPERATION_ENABLE)
00269   {
00270     LOGGING_INFO (CanOpen, "Quick stop of node " << m_node_id << " requested!" << endl);
00271     std::string identifier = "control_word";
00272     ds402::Controlword word;
00273     try
00274     {
00275       word.all = getRPDOValue<uint16_t> (identifier);
00276       word.bit.enable_voltage = 1;
00277       word.bit.quick_stop = 0;
00278       word.bit.reset_fault = 0;
00279       word.bit.halt = 1;
00280       setRPDOValue (identifier, word.all);
00281     }
00282     catch (const PDOException& e)
00283     {
00284       LOGGING_WARNING_C (CanOpen, DS402Node, "Could not find a PDO for the control word. Using slower SDO calls." << endl);
00285       word.bit.enable_voltage = 1;
00286       word.bit.quick_stop = 0;
00287       word.bit.reset_fault = 0;
00288       word.bit.halt = 1;
00289 
00290       m_sdo.download(false, ds402::ID_CONTROL_WORD, 0x00, word.all);
00291     }
00292   }
00293   m_expected_ds402_state = ds402::STATE_QUICKSTOP_ACTIVE;
00294 }
00295 
00296 void DS402Node::stopNode()
00297 {
00298   quickStop();
00299 }
00300 
00301 
00302 void DS402Node::initDS402State (const ds402::eState& requested_state)
00303 {
00304   LOGGING_DEBUG (CanOpen, "State " << deviceStatusString(requested_state) << " requested for node "
00305   << static_cast<int>(m_node_id) << "" << endl);
00306   if (requested_state == ds402::STATE_START ||
00307       requested_state == ds402::STATE_NOT_READY_TO_SWITCH_ON ||
00308       requested_state == ds402::STATE_FAULT_REACTION_ACTIVE)
00309   {
00310     LOGGING_ERROR_C (CanOpen, DS402Node, "Illegal target state requested. Requested state was: " <<
00311                      ds402::deviceStatusString(requested_state) << ", however that state cannot be entered manually!" << endl);
00312     return;
00313   }
00314 
00315   // get current state
00316   ds402::Statusword status_word;
00317   status_word.all = getTPDOValue<uint16_t>("status_word");
00318   ds402::eState current_state = ds402::stateFromStatusword(status_word);
00319 
00320 
00321   if (current_state == ds402::STATE_FAULT)
00322   {
00323     bool reset_fault_successful = false;
00324     for (size_t i = 0; i < 5; ++i)
00325     {
00326       reset_fault_successful = resetFault();
00327       if (reset_fault_successful)
00328       {
00329         break;
00330       }
00331     }
00332 
00333     if (!reset_fault_successful)
00334     {
00335       LOGGING_INFO (CanOpen, "Unable to reset from fault state after 5 tries. " <<
00336                              "Trying to do a hard reset on the device." << endl);
00337       m_nmt.stop();
00338       usleep(100000);
00339       m_nmt.start();
00340 
00341       if (!resetFault())
00342       {
00343         std::stringstream ss;
00344         ss << "Could not perform fault reset for node "
00345            << static_cast<int>(m_node_id)
00346            << " after multiple tries. "
00347            << "Is the device hard-disabled?"
00348            << "\nOtherwise: Have you tried turning it off and on again?";
00349         throw (DeviceException(ss.str()));
00350       }
00351     }
00352 
00353     m_sdo.upload(false, 0x6041, 0x0, status_word.all );
00354     current_state = ds402::stateFromStatusword(status_word);
00355   }
00356 
00357   // To prevent a deadlock if something goes wrong with state changes, the number of changes is limited
00358   size_t num_transitions = 0;
00359 
00360   while (current_state != requested_state && num_transitions < m_max_number_of_state_transitions)
00361   {
00362     LOGGING_DEBUG (CanOpen, "Node " << static_cast<int>(m_node_id) << " currently in state " <<
00363      deviceStatusString(current_state) << endl);
00364     switch (current_state)
00365     {
00366       case ds402::STATE_SWITCH_ON_DISABLED:
00367       {
00368         if (requested_state > ds402::STATE_SWITCH_ON_DISABLED)
00369         {
00370           doDS402StateTransition(ds402::STATE_TRANS_SHUTDOWN); // transition 2
00371           m_expected_ds402_state = ds402::STATE_READY_TO_SWITCH_ON;
00372         }
00373         break;
00374       }
00375       case ds402::STATE_READY_TO_SWITCH_ON:
00376       {
00377         if (requested_state > ds402::STATE_READY_TO_SWITCH_ON)
00378         {
00379           doDS402StateTransition(ds402::STATE_TRANS_SWITCH_ON); // transition 3
00380           m_expected_ds402_state = ds402::STATE_SWITCHED_ON;
00381         }
00382         else
00383         {
00384           doDS402StateTransition(ds402::STATE_TRANS_QUICK_STOP); // transition 7
00385           m_expected_ds402_state = ds402::STATE_SWITCH_ON_DISABLED;
00386         }
00387         break;
00388       }
00389       case ds402::STATE_SWITCHED_ON:
00390       {
00391         if (requested_state > ds402::STATE_SWITCHED_ON)
00392         {
00393           doDS402StateTransition(ds402::STATE_TRANS_ENABLE_OPERATION); // transition 4
00394           m_expected_ds402_state = ds402::STATE_OPERATION_ENABLE;
00395         }
00396         else if (requested_state == ds402::STATE_SWITCH_ON_DISABLED)
00397         {
00398           doDS402StateTransition(ds402::STATE_TRANS_INITIALIZE); // transition 10
00399           m_expected_ds402_state = ds402::STATE_SWITCH_ON_DISABLED;
00400         }
00401         else
00402         {
00403           doDS402StateTransition(ds402::STATE_TRANS_SHUTDOWN); // transition 6
00404           m_expected_ds402_state = ds402::STATE_READY_TO_SWITCH_ON;
00405         }
00406         break;
00407       }
00408       case ds402::STATE_OPERATION_ENABLE:
00409       {
00410         if (requested_state > ds402::STATE_OPERATION_ENABLE)
00411         {
00412           doDS402StateTransition(ds402::STATE_TRANS_QUICK_STOP); // transition 11
00413           m_expected_ds402_state = ds402::STATE_QUICKSTOP_ACTIVE;
00414         }
00415         else if (requested_state == ds402::STATE_SWITCHED_ON)
00416         {
00417           doDS402StateTransition(ds402::STATE_TRANS_SWITCH_ON); // transition 5
00418           m_expected_ds402_state = ds402::STATE_SWITCHED_ON;
00419         }
00420         else if (requested_state == ds402::STATE_READY_TO_SWITCH_ON)
00421         {
00422           doDS402StateTransition(ds402::STATE_TRANS_SHUTDOWN); // transition 8
00423           m_expected_ds402_state = ds402::STATE_READY_TO_SWITCH_ON;
00424         }
00425         else if (requested_state == ds402::STATE_SWITCH_ON_DISABLED)
00426         {
00427           doDS402StateTransition(ds402::STATE_TRANS_INITIALIZE); // transition 9
00428           m_expected_ds402_state = ds402::STATE_SWITCH_ON_DISABLED;
00429         }
00430         break;
00431       }
00432       case ds402::STATE_QUICKSTOP_ACTIVE:
00433       {
00434         // Query quick_stop option code. If it is 5,6,7 or 8, then we can perform this transition
00435         // However, this is optional, so we have to check if it's possible.
00436         int16_t quick_stop_option_code = 0;
00437         try
00438         {
00439           m_sdo.download(false, 0x605a, 0, quick_stop_option_code);
00440         }
00441         catch (const ProtocolException& e)
00442         {
00443           LOGGING_WARNING (CanOpen, "Caught error while downloading quick-stop-option code. "
00444                                     << "Probably it is not implemented, as it is optional. "
00445                                     << "Will carry on assuming quick-stop-option-code = 0.\n"
00446                                     << "Error was: " << e.what() << endl
00447           );
00448         }
00449         if (quick_stop_option_code >=5 && quick_stop_option_code <=8 &&
00450             requested_state == ds402::STATE_OPERATION_ENABLE)
00451         {
00452           doDS402StateTransition(ds402::STATE_TRANS_ENABLE_OPERATION); // transition 16
00453           m_expected_ds402_state = ds402::STATE_OPERATION_ENABLE;
00454         }
00455         else
00456         {
00457           doDS402StateTransition(ds402::STATE_TRANS_INITIALIZE); // transition 12
00458           m_expected_ds402_state = ds402::STATE_SWITCH_ON_DISABLED;
00459         }
00460         break;
00461       }
00462       case ds402::STATE_FAULT:
00463       {
00464         LOGGING_ERROR_C (CanOpen, DS402Node, "Landed in FAULT state while performing transition to " <<
00465                                              "state " << ds402::deviceStatusString(requested_state) <<
00466                                              ". Starting all over again..." << endl);
00467         break;
00468       }
00469       case ds402::STATE_START:
00470       case ds402::STATE_FAULT_REACTION_ACTIVE:
00471       case ds402::STATE_NOT_READY_TO_SWITCH_ON:
00472       {
00473         // nothing to do here.
00474         break;
00475       }
00476       default:
00477       {
00478         // This should not happen
00479         LOGGING_ERROR_C (CanOpen, DS402Node, "Landed in unknown state while performing transition to " <<
00480                                              "state " << ds402::deviceStatusString(requested_state) <<
00481                                              ". Aborting state transition." << endl);
00482         break;
00483       }
00484     }
00485 
00486     // now update the current state via SDO
00487     m_sdo.upload(false, 0x6041, 0x0, status_word.all );
00488     current_state = ds402::stateFromStatusword(status_word);
00489 
00490     LOGGING_INFO (CanOpen, "Node " << static_cast<int>(m_node_id) << " reached state " <<
00491      deviceStatusString(current_state) << endl);
00492     m_current_ds402_state = current_state;
00493     setTPDOValue("status_word", status_word.all);
00494 
00495     num_transitions++;
00496   }
00497   m_expected_ds402_state = m_current_ds402_state;
00498 
00499 }
00500 
00501 
00502 void DS402Node::home()
00503 {
00504   if (m_homing_method == 0)
00505   {
00506     LOGGING_WARNING (CanOpen, "Homing method for node " << static_cast<int>(m_node_id) <<
00507       " is not set. Aborting homing now." << endl);
00508     return;
00509   }
00510 
00511   LOGGING_INFO (CanOpen, "Starting homing for node " << static_cast<int>(m_node_id) << endl);
00512 
00513   setModeOfOperation(ds402::MOO_HOMING_MODE);
00514 
00515   initDS402State(ds402::STATE_OPERATION_ENABLE);
00516 
00517   ds402::Controlword controlword;
00518   controlword.all = getRPDOValue<uint16_t>("control_word");
00519 
00520   // home operation start
00521   controlword.bit.operation_mode_specific_0 = 1;
00522   controlword.bit.halt = 0;
00523 
00524   // activate the homing mode
00525   m_sdo.download(false, ds402::ID_CONTROL_WORD, 0x0, controlword.all);
00526 
00527   ds402::Statusword statusword;
00528   bool homing_attained = false;
00529   bool homing_error = false;
00530   while (!homing_attained)
00531   {
00532     m_sdo.upload(false, ds402::ID_STATUS_WORD, 0, statusword.all);
00533     homing_attained = statusword.bit.operation_mode_specific_0;
00534     homing_error = statusword.bit.operation_mode_specific_1;
00535 
00536     if (homing_error)
00537     {
00538       std::stringstream ss;
00539       ss << "Homing of node " << static_cast<int>(m_node_id) << " failed.";
00540       throw DeviceException(ss.str());
00541     }
00542 
00543     if (homing_attained)
00544     {
00545       // We're done homing
00546       LOGGING_INFO (CanOpen, "Done homing for node " << static_cast<int>(m_node_id) << endl);
00547       break;
00548     }
00549 
00550     usleep(100000); // wait 100 ms
00551   }
00552 }
00553 
00554 void DS402Node::printStatus()
00555 {
00556   ds402::Statusword statusword;
00557   statusword.all = getTPDOValue<uint16_t> ("status_word");
00558   ds402::eState state = stateFromStatusword(statusword);
00559   std::stringstream ss;
00560   ss << "Device " << static_cast<int>(m_node_id) << " status: " <<
00561    binaryString(statusword.all) << "\n(state " << ds402::deviceStatusString(state)
00562    << ")" << std::endl;
00563   ss << "Fault: " << statusword.bit.fault << std::endl;
00564   ss << "Switched on: " << statusword.bit.switched_on << std::endl;
00565   ss << "Operation enabled: " << statusword.bit.operation_enabled << std::endl;
00566   ss << "Voltage enabled: " << statusword.bit.voltage_enabled << std::endl;
00567   ss << "Quick stop active: " << statusword.bit.quick_stop << std::endl;
00568   ss << "Target reached: " << statusword.bit.target_reached << std::endl;
00569 
00570   // add Bits 12 and 13 which are op_mode specific
00571   ss << operationModeSpecificStatus(statusword);
00572 
00573   LOGGING_INFO_C (CanOpen, DS402Node, ss.str() << endl);
00574 }
00575 
00576 std::string DS402Node::operationModeSpecificStatus (const ds402::Statusword& statusword)
00577 {
00578   std::stringstream ss;
00579   switch (m_op_mode)
00580   {
00581     case ds402::MOO_PROFILE_POSITION_MODE:
00582     {
00583       ss << "Set-point acknowledge: " << statusword.bit.operation_mode_specific_0 << std::endl;
00584       ss << "Following error: " << statusword.bit.operation_mode_specific_1 << std::endl;
00585       break;
00586     }
00587     case ds402::MOO_VELOCITY_MODE:
00588     {
00589       break;
00590     }
00591     case ds402::MOO_PROFILE_VELOCITY_MODE:
00592     {
00593       ss << "Speed: " << statusword.bit.operation_mode_specific_0 << std::endl;
00594       ss << "Max slippage error: " << statusword.bit.operation_mode_specific_1 << std::endl;
00595       break;
00596     }
00597     case ds402::MOO_PROFILE_TORQUE_MODE:
00598     {
00599       break;
00600     }
00601     case ds402::MOO_HOMING_MODE:
00602     {
00603       ss << "Homing attained: " << statusword.bit.operation_mode_specific_0 << std::endl;
00604       ss << "Homing error: " << statusword.bit.operation_mode_specific_1 << std::endl;
00605       break;
00606     }
00607     case ds402::MOO_INTERPOLATED_POSITION_MODE:
00608     {
00609       ss << "Interpolated position mode active: " << statusword.bit.operation_mode_specific_0 << std::endl;
00610       break;
00611     }
00612     case ds402::MOO_CYCLIC_SYNC_POSITION_MODE:
00613     {
00614       break;
00615     }
00616     case ds402::MOO_CYCLIC_SYNC_VELOCITY_MODE:
00617     {
00618       break;
00619     }
00620     case ds402::MOO_CYCLIC_SYNC_TORQUE_MODE:
00621     {
00622       break;
00623     }
00624     default:
00625     {
00626       // do nothing
00627       break;
00628     }
00629   }
00630 
00631   return ss.str();
00632 }
00633 
00634 void DS402Node::querySupportedDeviceModes()
00635 {
00636   m_sdo.upload(false, 0x6502, 0, m_supported_modes.all);
00637 }
00638 
00639 void DS402Node::initNode()
00640 {
00641   setDefaultPDOMapping(PDO_MAPPING_PROFILE_POSITION_MODE);
00642   DS301Node::initNode();
00643   querySupportedDeviceModes();
00644 
00645   boost::function <void()> f = boost::bind(&DS402Node::onStatusWordUpdate, this);
00646   registerPDONotifyCallback("status_word", f);
00647 
00648   setModeOfOperation(ds402::MOO_PROFILE_POSITION_MODE);
00649 }
00650 
00651 bool DS402Node::isModeSupported (const ds402::eModeOfOperation op_mode)
00652 {
00653   int8_t mode_int = op_mode;
00654   uint32_t bitmask_mode = 0x01 << (mode_int-1);
00655 
00656   if ((m_supported_modes.all & bitmask_mode) != bitmask_mode)
00657   {
00658     return false;
00659   }
00660 
00661   return true;
00662 }
00663 
00664 void DS402Node::printSupportedModesOfOperation()
00665 {
00666   std::stringstream ss;
00667   ss << "Modes of operation supported by device " << static_cast<int>(m_node_id) << std::endl;
00668   if (m_supported_modes.bit.profile_position_mode)
00669   {
00670     ss << "Profile position mode" << std::endl;
00671   }
00672   if (m_supported_modes.bit.velocity_mode)
00673   {
00674     ss << "Velocity mode" << std::endl;
00675   }
00676   if (m_supported_modes.bit.profile_velocity_mode)
00677   {
00678     ss << "Profile velocity mode" << std::endl;
00679   }
00680   if (m_supported_modes.bit.profile_torque_mode)
00681   {
00682     ss << "Profile torque mode" << std::endl;
00683   }
00684   if (m_supported_modes.bit.homing_mode)
00685   {
00686     ss << "Homing mode" << std::endl;
00687   }
00688   if (m_supported_modes.bit.interpolated_position_mode)
00689   {
00690     ss << "Interpolated position mode" << std::endl;
00691   }
00692   if (m_supported_modes.bit.cyclic_sync_position_mode)
00693   {
00694     ss << "Cyclic sync position mode" << std::endl;
00695   }
00696   if (m_supported_modes.bit.cyclic_sync_velocity_mode)
00697   {
00698     ss << "Cyclic sync velocity mode" << std::endl;
00699   }
00700   if (m_supported_modes.bit.cyclic_sync_torque_mode)
00701   {
00702     ss << "Cyclic sync torque mode" << std::endl;
00703   }
00704 
00705   LOGGING_INFO (CanOpen, ss.str() << endl);
00706 }
00707 
00708 
00709 bool DS402Node::setModeOfOperation (const ds402::eModeOfOperation op_mode)
00710 {
00711   // Changes of opMode are always possible. So make sure, the motor is standing while changing the opMode.
00712   if (m_current_ds402_state == ds402::STATE_OPERATION_ENABLE)
00713   {
00714     closeBrakes();
00715   }
00716 
00717   if (op_mode != ds402::MOO_HOMING_MODE &&
00718       op_mode != ds402::MOO_INTERPOLATED_POSITION_MODE &&
00719       op_mode != ds402::MOO_PROFILE_POSITION_MODE
00720      )
00721   {
00722     LOGGING_ERROR_C (CanOpen, DS402Node, "Requested to switch to mode " <<
00723     operationModeString(op_mode) << " for node " <<
00724     static_cast<int>(m_node_id) << ", which is currently not supported." << endl);
00725     return false;
00726   }
00727 
00728   if (!isModeSupported(op_mode))
00729   {
00730     LOGGING_ERROR_C (CanOpen, DS402Node, "The requested mode: " <<
00731       operationModeString(op_mode) <<
00732       " is not supported by the device " <<
00733       static_cast<int>(m_node_id) << "." << endl);
00734     return false;
00735   }
00736 
00737   if (op_mode == ds402::MOO_INTERPOLATED_POSITION_MODE)
00738   {
00739     configureInterpolationCycleTime();
00740     configureInterpolationData (0, 0, 4);
00741   }
00742 
00743   bool success = true;
00744 
00745   int8_t mode_int = op_mode;
00746   try
00747   {
00748     success = m_sdo.download(false, 0x6060, 0, mode_int);
00749   }
00750   catch (const std::exception& e)
00751   {
00752     LOGGING_ERROR_C (CanOpen, DS402Node, e.what() << endl);
00753   }
00754   if (success)
00755   {
00756     m_op_mode = op_mode;
00757     LOGGING_INFO (CanOpen, "Initialized mode " << ds402::operationModeString(op_mode) <<
00758      " for node " << m_node_id << endl);
00759   }
00760 
00761   return success;
00762 }
00763 
00764 void DS402Node::configureInterpolationCycleTime (const uint8_t interpolation_cycle_time_ms)
00765 {
00766   if (interpolation_cycle_time_ms != 0)
00767   {
00768     m_interpolation_cycle_time_ms = interpolation_cycle_time_ms;
00769   }
00770   m_sdo.download(false, 0x60c2, 0x01, m_interpolation_cycle_time_ms);
00771   int8_t magnitude = -3; // milliseconds
00772   m_sdo.download(false, 0x60c2, 0x02, magnitude);
00773 }
00774 
00775 void DS402Node::setupProfilePositionMode (const ds402::ProfilePositionModeConfiguration& config)
00776 {
00777   configureProfileAcceleration(config.profile_acceleration * m_transmission_factor);
00778 
00779 
00780   // If only an acceleration parameter is given, it will be used for deceleration, as well.
00781   float deceleration = config.profile_deceleration;
00782   if (config.profile_deceleration == 0)
00783   {
00784     deceleration = config.profile_acceleration;
00785   }
00786 
00787 
00788   // Some devices don't support setting deceleration parameter. This catches that.
00789   try
00790   {
00791     configureProfileDeceleration(deceleration * m_transmission_factor);
00792   }
00793   catch (const ProtocolException& e)
00794   {
00795     LOGGING_DEBUG (CanOpen, "Failed to set the profile deceleration for node " << m_node_id << ". " <<
00796                            " Some devices do not support setting this, but use the acceleration value for deceleration, as well. So you might want to ignore this message. " <<
00797                            "The error was: " << e.what() << endl
00798     );
00799   }
00800 
00801   // Some devices don't support setting it and 0 is the default
00802   if (config.motion_profile_type != 0)
00803   {
00804     configureMotionProfileType(config.motion_profile_type * m_transmission_factor);
00805   }
00806   configureProfileVelocity(config.profile_velocity * m_transmission_factor);
00807 
00808   m_ppm_config = config;
00809 }
00810 
00811 void DS402Node::setupHomingMode (const ds402::HomingModeConfiguration& config)
00812 {
00813   configureHomingSpeeds (config.homing_speed_low, config.homing_speed_high);
00814   configureHomingMethod (config.homing_method);
00815 }
00816 
00817 void DS402Node::setupProfileVelocityMode (const ds402::ProfileVelocityModeConfiguration& config)
00818 {
00819   configureSensorSelectionCode (config.sensor_selection_code);
00820 }
00821 
00822 void DS402Node::setupProfileTorqueMode (const ds402::ProfileTorqueModeConfiguration& config)
00823 {
00824   configureTorqueSlope (config.torque_slope);
00825   configureTorqueProfileType (config.torque_profile_type);
00826 }
00827 
00828 bool DS402Node::resetFault ()
00829 {
00830   ds402::Statusword status_word;
00831   status_word.all = getTPDOValue<uint16_t>("status_word");
00832   ds402::eState current_state = ds402::stateFromStatusword(status_word);
00833 
00834   if (current_state != ds402::STATE_FAULT)
00835   {
00836     LOGGING_INFO_C (CanOpen, DS402Node, "Requested resetFault action, but device is currently " <<
00837     "not in state FAULT. Instead it is in state " << deviceStatusString (current_state) <<
00838     ". Not doing anything here." << endl);
00839     return true;
00840   }
00841 
00842   // clear the error register
00843   m_emcy->clearErrorHistory(m_sdo);
00844 
00845   // enter SWITCH_ON_DISABLED state
00846   doDS402StateTransition(ds402::STATE_TRANS_FAULT_RESET); // transition 15
00847 
00848   usleep (100000); // wait 100ms
00849   status_word.all = getTPDOValue<uint16_t>("status_word");
00850   current_state = ds402::stateFromStatusword(status_word);
00851 
00852   if (current_state != ds402::STATE_SWITCH_ON_DISABLED)
00853   {
00854     LOGGING_ERROR_C (CanOpen, DS402Node, "Could not perform fault reset for node " <<
00855       m_node_id << ". Possibly the reason for entering the fault state still exists." <<
00856       endl
00857     );
00858     return false;
00859   }
00860   return true;
00861 }
00862 
00863 
00864 void DS402Node::configureMaxAcceleration (const uint32_t acceleration)
00865 {
00866   m_sdo.download(false, 0x60c5, 0, acceleration);
00867 
00868   LOGGING_INFO_C (CanOpen, DS402Node, "Maximum acceleration for node " << m_node_id << " written." << endl);
00869 }
00870 
00871 void DS402Node::configureMaxDeceleration (const uint32_t deceleration)
00872 {
00873   m_sdo.download(false, 0x60c6, 0, deceleration);
00874   LOGGING_INFO_C (CanOpen, DS402Node, "Maximum deceleration for node " << m_node_id << " written." << endl);
00875 }
00876 
00877 void DS402Node::configureProfileVelocity (const uint32_t velocity)
00878 {
00879  m_sdo.download(false, 0x6081, 0, velocity);
00880  LOGGING_INFO_C (CanOpen, DS402Node, "Profile velocity for node " << m_node_id << " written with value " <<
00881                                         velocity << "." << endl);
00882 }
00883 
00884 
00885 void DS402Node::configureProfileAcceleration (const uint32_t acceleration)
00886 {
00887   m_sdo.download(false, 0x6083, 0, acceleration);
00888   LOGGING_INFO_C (CanOpen, DS402Node, "Profile acceleration for node " << m_node_id << " written with value " <<
00889                                         acceleration << "." << endl);
00890 }
00891 
00892 void DS402Node::configureProfileDeceleration (const uint32_t deceleration)
00893 {
00894   m_sdo.download(false, 0x6084, 0, deceleration);
00895 }
00896 
00897 void DS402Node::configureQuickStopDeceleration (const uint32_t deceleration)
00898 {
00899   m_sdo.download(false, 0x6085, 0, deceleration);
00900   LOGGING_INFO_C (CanOpen, DS402Node, "Quick Stop deceleration for node " << m_node_id << " written with value " <<
00901                                         deceleration << "." << endl);
00902 }
00903 
00904 void DS402Node::configureMotionProfileType (const int16_t motion_type)
00905 {
00906   m_sdo.download(false, 0x6086, 0, motion_type);
00907   LOGGING_INFO_C (CanOpen, DS402Node, "Motion profile type for node " << m_node_id << " written with value " <<
00908                                         motion_type << "." << endl);
00909 }
00910 
00911 
00912 void DS402Node::configureHomingSpeeds (const uint32_t low_speed, const uint32_t high_speed)
00913 {
00914   m_sdo.download(false, 0x6099, 1, high_speed);
00915   m_sdo.download(false, 0x6099, 2, low_speed);
00916 
00917   LOGGING_INFO_C (CanOpen, DS402Node, "Homing speeds for node " << m_node_id << " written." << endl);
00918 
00919 }
00920 
00921 void DS402Node::configureHomingAcceleration (const uint32_t acceleration)
00922 {
00923   m_sdo.download(false, 0x609A, 0, acceleration);
00924   LOGGING_INFO_C (CanOpen, DS402Node, "Homing acceleration for node " << m_node_id << " written." << endl);
00925 }
00926 
00927 void DS402Node::configureHomingMethod (const int8_t homing_method)
00928 {
00929   m_sdo.download(false, 0x6098, 0, homing_method);
00930   LOGGING_INFO_C (CanOpen, DS402Node, "Homing method for node " << m_node_id << " written." << endl);
00931   m_homing_method = homing_method;
00932 }
00933 
00934 void DS402Node::configureSensorSelectionCode (const int16_t sensor_selection_code)
00935 {
00936   m_sdo.download(false, 0x606a, 0, sensor_selection_code);
00937   LOGGING_INFO_C (CanOpen, DS402Node, "Sensor selection code for node " << m_node_id << " written." << endl);
00938 }
00939 
00940 void DS402Node::configureTorqueProfileType (const int16_t torque_profile_type)
00941 {
00942   m_sdo.download(false, 0x6088, 0, torque_profile_type);
00943   LOGGING_INFO_C (CanOpen, DS402Node, "Torque profile type for node " << m_node_id << " written." << endl);
00944 }
00945 
00946 void DS402Node::configureTorqueSlope (const uint32_t torque_slope)
00947 {
00948   m_sdo.download(false, 0x6087, 0, torque_slope);
00949   LOGGING_INFO_C (CanOpen, DS402Node, "Torque slope for node " << m_node_id << " written." << endl);
00950 }
00951 
00952 void DS402Node::configureInterpolationData (const uint8_t buffer_organization,
00953                                             const int16_t interpolation_type,
00954                                             const uint8_t size_of_data_record)
00955 {
00956   uint8_t data8 = 0;
00957   // Reset the buffer
00958   m_sdo.download(false, 0x60c4, 6, data8);
00959 
00960   // Enable the buffer again
00961   data8 = 1;
00962   m_sdo.download(false, 0x60c4, 6, data8);
00963 
00964   // set linear interpolation
00965   int16_t data16 = 0;
00966   m_sdo.download(false, 0x60c0, 0, data16);
00967 
00968   m_sdo.download(false, 0x60c4, 3, buffer_organization);
00969   m_sdo.download(false, 0x60c4, 5, size_of_data_record);
00970 
00971   LOGGING_DEBUG_C (CanOpen, DS402Node, "Interpolation data for node " << m_node_id << " written." << endl);
00972 }
00973 
00974 void DS402Node::doDS402StateTransition (const ds402::eStateTransission transition)
00975 {
00976   LOGGING_DEBUG_C (CanOpen, DS402Node, "Requested DS402 state transition " << transition << endl);
00977   // create according control word and send it via SDO, update it in the PDO cache
00978   std::string identifier = "control_word";
00979   ds402::Controlword word;
00980   word.all = getRPDOValue<uint16_t> (identifier);
00981 
00982   switch (transition)
00983   {
00984     case ds402::STATE_TRANS_INITIALIZE: // 1, 7, 9, 10, 12
00985     {
00986       word.bit.reset_fault = 0;
00987       word.bit.enable_voltage = 0;
00988       word.bit.operation_mode_specific_0 = 0;
00989       break;
00990     }
00991     case ds402::STATE_TRANS_SHUTDOWN: // 2, 6, 8
00992     {
00993       word.bit.reset_fault = 0;
00994       word.bit.quick_stop = 1;
00995       word.bit.enable_voltage = 1;
00996       word.bit.switch_on = 0;
00997       word.bit.operation_mode_specific_0 = 0;
00998       break;
00999     }
01000     case ds402::STATE_TRANS_SWITCH_ON: // 3, 5
01001     {
01002       word.bit.reset_fault = 0;
01003       word.bit.enable_operation = 0;
01004       word.bit.quick_stop = 1;
01005       word.bit.enable_voltage = 1;
01006       word.bit.switch_on = 1;
01007       word.bit.halt = 1;
01008       break;
01009     }
01010     case ds402::STATE_TRANS_ENABLE_OPERATION: // 4, 16
01011     {
01012       word.bit.reset_fault = 0;
01013       word.bit.enable_operation = 1;
01014       word.bit.quick_stop = 1;
01015       word.bit.enable_voltage = 1;
01016       word.bit.switch_on = 1;
01017       word.bit.halt = 0;
01018       break;
01019     }
01020     case ds402::STATE_TRANS_QUICK_STOP: // 11
01021     {
01022       word.bit.reset_fault = 0;
01023       word.bit.quick_stop = 0;
01024       word.bit.enable_voltage = 1;
01025       word.bit.halt = 1;
01026       break;
01027     }
01028     case ds402::STATE_TRANS_FAULT_RESET: // 15
01029     {
01030       word.bit.reset_fault = 1;
01031       break;
01032     }
01033     default:
01034     {
01035       std::stringstream ss;
01036       ss << "Illegal DS402 state transition requested: " << transition;
01037       throw ProtocolException (ds402::ID_CONTROL_WORD, 0x00, ss.str());
01038     }
01039   }
01040 
01041   // now send the controlword to the device
01042   m_sdo.download(false, ds402::ID_CONTROL_WORD, 0x0, word.all);
01043   LOGGING_DEBUG (CanOpen, "Sent controlword " << binaryString(word.all) << endl);
01044 
01045   // copy it to the pdo cache
01046   setRPDOValue(identifier, word.all);
01047 }
01048 
01049 void DS402Node::enableNode (const ds402::eModeOfOperation operation_mode)
01050 {
01051   m_nmt.start();
01052   if (m_current_ds402_state == ds402::STATE_FAULT)
01053   {
01054     resetFault();
01055   }
01056 
01057   if (operation_mode != ds402::MOO_RESERVED_0)
01058   {
01059     setModeOfOperation(operation_mode);
01060   }
01061   initDS402State(ds402::STATE_OPERATION_ENABLE);
01062   // Set current position to target position to prevent jumps
01063   double feedback = getTargetFeedback();
01064   setTarget(feedback);
01065 
01066   openBrakes();
01067 }
01068 
01069 void DS402Node::disableNode()
01070 {
01071   if (m_current_ds402_state == ds402::STATE_OPERATION_ENABLE)
01072   {
01073     closeBrakes();
01074   }
01075 
01076   initDS402State(ds402::STATE_SWITCHED_ON);
01077 }
01078 
01079 void DS402Node::openBrakes()
01080 {
01081   if (m_current_ds402_state == ds402::STATE_OPERATION_ENABLE)
01082   {
01083     ds402::Controlword controlword;
01084     controlword.all = getRPDOValue<uint16_t>("control_word");
01085 
01086 
01087     // If the requested mode is InterpolatedPositionMode, we enable interpolation here
01088     if (m_op_mode == ds402::MOO_INTERPOLATED_POSITION_MODE)
01089     {
01090       controlword.bit.operation_mode_specific_0 = 1;
01091       controlword.bit.halt = 0;
01092     }
01093     else if (m_op_mode == ds402::MOO_PROFILE_POSITION_MODE)
01094     {
01095       controlword.bit.operation_mode_specific_0 = 0; // Bit 4
01096 
01097       controlword.bit.operation_mode_specific_1 = !(m_ppm_config.change_set_immediately); // Bit 5: move to set point immediately
01098       controlword.bit.operation_mode_specific_2 = m_ppm_config.use_relative_targets; // Bit 6
01099       controlword.bit.reserved_0                = m_ppm_config.use_blending; // Bit 9: use blending
01100       controlword.bit.halt                     = 0;
01101     }
01102     else
01103     {
01104       controlword.bit.operation_mode_specific_0 = 0;
01105       controlword.bit.halt = 0;
01106     }
01107     setRPDOValue("control_word", controlword.all);
01108   }
01109   else
01110   {
01111     LOGGING_ERROR (CanOpen, "OpenBrakes called while not in OPERATION_ENABLE state. Will do nothing" << endl);
01112   }
01113 }
01114 
01115 
01116 void DS402Node::closeBrakes()
01117 {
01118   if (m_current_ds402_state == ds402::STATE_OPERATION_ENABLE)
01119   {
01120     ds402::Controlword controlword;
01121     m_sdo.upload(false, 0x6040, 0, controlword.all);
01122     // If the requested mode is InterpolatedPositionMode, we disable interpolation here
01123     if (m_op_mode == ds402::MOO_INTERPOLATED_POSITION_MODE)
01124     {
01125       controlword.bit.operation_mode_specific_0 = 0;
01126     }
01127     controlword.bit.halt = 1;
01128     setRPDOValue("control_word", controlword.all);
01129   }
01130   else
01131   {
01132     LOGGING_ERROR (CanOpen, "CloseBrakes called while not in OPERATION_ENABLE state. Will do nothing" << endl);
01133   }
01134 }
01135 
01136 void DS402Node::onStatusWordUpdate()
01137 {
01138   ds402::Statusword statusword;
01139   statusword.all = getTPDOValue<uint16_t>("status_word");
01140 
01141   // check if DS402 status  matches the one from the device
01142 
01143   ds402::eState device_state =  ds402::stateFromStatusword(statusword);
01144   if (m_current_ds402_state != device_state)
01145   {
01146     if (m_expected_ds402_state != device_state)
01147     {
01148       LOGGING_WARNING (CanOpen, "The device " << m_node_id <<
01149                                 " has switched to state " <<
01150                                 ds402::deviceStatusString(device_state) <<
01151                                 " without host request. " <<
01152                                 "The controller will adapt the device's status." << endl
01153       );
01154     }
01155     m_current_ds402_state = device_state;
01156   }
01157 }
01158 
01159 bool DS402Node::isTargetReached()
01160 {
01161   ds402::Statusword statusword;
01162   statusword.all = getTPDOValue<uint16_t>("status_word");
01163 
01164   return statusword.bit.target_reached;
01165 }
01166 
01167 ds402::Statusword DS402Node::getStatus()
01168 {
01169   ds402::Statusword word;
01170   word.all =  getTPDOValue<uint16_t>("status_word");
01171   return word;
01172 }
01173 
01174 
01175 
01176 }}//end of NS


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Sun May 22 2016 03:30:56