SchunkPowerBallNode.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00003 // This file is part of the SCHUNK Canopen Driver suite.
00004 //
00005 // This program is free software licensed under the LGPL
00006 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00007 // You can find a copy of this license in LICENSE folder in the top
00008 // directory of the source code.
00009 //
00010 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00011 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00012 // -- END LICENSE BLOCK ------------------------------------------------
00013 //----------------------------------------------------------------------
00020 //----------------------------------------------------------------------
00021 
00022 #include "SchunkPowerBallNode.h"
00023 
00024 #include "sync.h"
00025 
00026 #include "Logging.h"
00027 #include "exceptions.h"
00028 
00029 namespace icl_hardware {
00030 namespace canopen_schunk {
00031 
00032 int boolify( int v )
00033 {
00034     return (v!=0);
00035 }
00036 
00037 SchunkPowerBallNode::SchunkPowerBallNode (const uint8_t node_id, const CanDevPtr& can_device, HeartBeatMonitor::Ptr heartbeat_monitor)
00038   : DS402Node(node_id, can_device, heartbeat_monitor)
00039 {
00040   m_homing_method = 33;
00041   m_transmission_factor = RAD_TO_STEPS_FACTOR;
00042 }
00043 
00044 void SchunkPowerBallNode::initNode()
00045 {
00046   icl_hardware::canopen_schunk::DS402Node::initNode();
00047   m_nmt.start();
00048   commutationSearch();
00049   setModeOfOperation(ds402::MOO_PROFILE_POSITION_MODE);
00050 }
00051 
00052 void SchunkPowerBallNode::setDefaultPDOMapping (const DS402Node::eDefaultPDOMapping mapping)
00053 {
00054   PDO::MappingConfigurationList rpdo_mappings;
00055   PDO::MappingConfigurationList tpdo_mappings;
00056   switch (mapping)
00057   {
00058     case PDO_MAPPING_CONTROLWORD_STATUSWORD:
00059     {
00060       // Map control and status word to the first PDO (receive and transmit respectively)
00061       rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00062       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6041, 0, 16, "status_word"));
00063       initPDOMappingSingle (rpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00064       initPDOMappingSingle (tpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::TRANSMIT_PDO);
00065       break;
00066     }
00067     case PDO_MAPPING_INTERPOLATED_POSITION_MODE:
00068     {
00069       // Map control and status word to the first PDO (receive and transmit respectively)
00070       rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00071       rpdo_mappings.push_back(PDO::MappingConfiguration(0x60B2, 0, 16, "torque_offset"));
00072       rpdo_mappings.push_back(PDO::MappingConfiguration(0x60C1, 1, 32, "interpolation_buffer"));
00073 
00074       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6041, 0, 16, "status_word"));
00075       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6077, 0, 16, "measured_torque"));
00076       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6064, 0, 32, "measured_position"));
00077 
00078       initPDOMappingSingle (rpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00079       initPDOMappingSingle (tpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::TRANSMIT_PDO);
00080       break;
00081     }
00082     case PDO_MAPPING_PROFILE_POSITION_MODE:
00083     {
00084       // We map the interpolated position buffer as well, as it is needed for commutation
00085       rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00086       rpdo_mappings.push_back(PDO::MappingConfiguration(0x60B2, 0, 16, "torque_offset"));
00087       rpdo_mappings.push_back(PDO::MappingConfiguration(0x60C1, 1, 32, "interpolation_buffer"));
00088 
00089       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6041, 0, 16, "status_word"));
00090       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6077, 0, 16, "measured_torque"));
00091       tpdo_mappings.push_back(PDO::MappingConfiguration(0x6064, 0, 32, "measured_position"));
00092 
00093       initPDOMappingSingle (rpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00094       initPDOMappingSingle (tpdo_mappings, 0, PDO::SYNCHRONOUS_CYCLIC, DS301Node::TRANSMIT_PDO);
00095 
00096       rpdo_mappings.clear();
00097       rpdo_mappings.push_back(PDO::MappingConfiguration(0x607A, 0, 32, "target_position"));
00098       initPDOMappingSingle(rpdo_mappings, 1, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO);
00099       break;
00100     }
00101     default:
00102     {
00103       break;
00104     }
00105   }
00106 }
00107 
00108 
00109 void SchunkPowerBallNode::commutationSearch()
00110 {
00111   LOGGING_INFO (CanOpen, "Commutation search for node  " << m_node_id << endl);
00112 
00113   bool calib_ok = CommutationCalibrated();
00114 
00115   // copy current position into the PDO buffer, as this will be read by the enable function
00116   int32_t current_position = 0;
00117   m_sdo.upload(false, 0x6064, 0x0, current_position);
00118   setTPDOValue("measured_position", current_position);
00119   LOGGING_INFO(CanOpen, "Initially, node is at position " << current_position << endl);
00120 
00121   // I'm not entirely sure why I need that, but I get an RPDO timeout error if I remove this
00122 
00123   sendSync(m_can_dev);
00124   bool disable_again = false;
00125 
00126   initDS402State(ds402::STATE_SWITCHED_ON);
00127   if (!calib_ok)
00128   {
00129     DS402Node::enableNode (ds402::MOO_INTERPOLATED_POSITION_MODE);
00130     disable_again = true;
00131     LOGGING_INFO (CanOpen, "Activated commutation search for node " << m_node_id << endl);
00132     // Make sure, the node is in interpolated position mode and enabled
00133   }
00134 
00135   size_t counter = 50;
00136 
00137   // Try commutation search for counter times
00138   while (!calib_ok && counter--)
00139   {
00140     usleep(100000); // sleep for 100 ms
00141     downloadPDOs();
00142     sendSync(m_can_dev);
00143     uploadPDOs();
00144     calib_ok = CommutationCalibrated();
00145   }
00146   if(!calib_ok)
00147   {
00148     LOGGING_ERROR (CanOpen, "Commutation of node "<< (int)(m_node_id) << " could not be ensured after 50 tries! Aborting... You probably should do a recalibration!" << endl);
00149   }
00150   else
00151   {
00152     if (disable_again)
00153     {
00154       disableNode();
00155     }
00156     LOGGING_INFO (CanOpen, "Commutation search for node " << m_node_id << " was successful!" << endl);
00157   }
00158 
00159 
00160   #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00161   if (m_ws_broadcaster)
00162   {
00163     m_ws_broadcaster->robot->setJointHomed(calib_ok, m_node_id);
00164     m_ws_broadcaster->robot->setInputToRadFactor(1.0 / RAD_TO_STEPS_FACTOR);
00165   }
00166   #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00167 
00168 }
00169 
00170 
00171 bool SchunkPowerBallNode::CommutationCalibrated()
00172 {
00173   uint8_t commutation_status;
00174 
00175   bool commutation_search_completed = true;
00176 
00177   //  - Bit 0 (Bit-Maske 0x01) mit der Bedeutung "commutation_search_completed"
00178   //  - Bit 1 (Bit-Maske 0x02) mit der Bedeutung "pseudo_absolute_position_verified"
00179   m_sdo.upload(false, 0x2050, 0, commutation_status);
00180 //   LOGGING_INFO(CanOpen, "Commutation status: " << hexToString(commutation_status) << endl);
00181   commutation_search_completed = commutation_status & (1 << 0);
00182 
00183   return commutation_search_completed;
00184 }
00185 
00186 
00187 
00188 void SchunkPowerBallNode::configureHomingMethod (const uint8_t homing_method)
00189 {
00190   LOGGING_ERROR (CanOpen, "configureHomingMethod called for a Schunk powerball node (id " <<
00191     m_node_id << "). " <<
00192     "However, the powerballs only support one homing mode so this request will be ignored." << endl
00193   );
00194 }
00195 
00196 void SchunkPowerBallNode::configureHomingSpeeds (const uint32_t low_speed, const uint32_t high_speed)
00197 {
00198   LOGGING_ERROR (CanOpen, "configureHomingSpeeds called for a Schunk powerball node (id " <<
00199     m_node_id << "). " <<
00200     "However, the powerballs do not allow that, so this request will be ignored." << endl
00201   );
00202 }
00203 
00204 void SchunkPowerBallNode::configureInterpolationData (const uint8_t buffer_organization,
00205                                                       const int16_t interpolation_type,
00206                                                       const uint8_t size_of_data_record)
00207 {
00208   DS402Node::configureInterpolationData (buffer_organization, interpolation_type, size_of_data_record);
00209 
00210   LOGGING_DEBUG (CanOpen, "Initializing interpolation data for Schunk power ball node with id " << m_node_id << endl);
00211 
00212   // Number of cycles that can be missed by the interpolator. If for more than this number of cycles
00213   // no new interpolation data is sent, the node will go into quick stop.
00214   // For hard realtime security use a value of 1 here.
00215   int8_t data8 = 20;
00216   m_sdo.download(false, 0x200e, 0, data8);
00217 }
00218 
00219 
00220 }} // end of NS


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