simple_example.cpp
Go to the documentation of this file.
00001 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00002 // This file is part of the SCHUNK Canopen Driver suite.
00003 //
00004 // This program is free software licensed under the LGPL
00005 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00006 // You can find a copy of this license in LICENSE folder in the top
00007 // directory of the source code.
00008 //
00009 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00010 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00011 // -- END LICENSE BLOCK ------------------------------------------------
00012 
00013 #include <icl_hardware_canopen/CanOpenController.h>
00014 #include <icl_hardware_canopen/DS301Group.h>
00015 #include <icl_hardware_canopen/SchunkPowerBallNode.h>
00016 #include <icl_hardware_canopen/DS402Group.h>
00017 #include <icl_hardware_canopen/exceptions.h>
00018 #include <icl_hardware_canopen/PDO.h>
00019 
00020 #include <icl_hardware_canopen/Logging.h>
00021 #include "../ds402.h"
00022 
00023 #include <cstdlib> // getenv
00024 #include <boost/filesystem.hpp>
00025 
00026 using namespace icl_hardware::canopen_schunk;
00027 
00028 int main (int argc, char* argv[])
00029 {
00030   // Initializing
00031   icl_core::logging::initialize(argc, argv);
00032 
00033   CanOpenController my_controller("auto");
00034 
00035   my_controller.addGroup<DS402Group>("arm");
00036   my_controller.addNode<SchunkPowerBallNode>(3, "arm");
00037   my_controller.addNode<SchunkPowerBallNode>(4, "arm");
00038   my_controller.addNode<SchunkPowerBallNode>(5, "arm");
00039   my_controller.addNode<SchunkPowerBallNode>(6, "arm");
00040   my_controller.addNode<SchunkPowerBallNode>(7, "arm");
00041   my_controller.addNode<SchunkPowerBallNode>(8, "arm");
00042 
00043   std::vector <uint8_t> output_data;
00044   sleep(5); // wait for 5 seconds
00045 
00046   boost::filesystem::path resources_path;
00047   char const* tmp = std::getenv("CANOPEN_RESOURCE_PATH");
00048   if (tmp == NULL)
00049   {
00050     LOGGING_WARNING_C(
00051         CanOpen,
00052         CanOpenController,
00053         "The environment variable 'CANOPEN_RESOURCE_PATH' could not be read. Using relative path 'resources/'" << endl);
00054     resources_path = boost::filesystem::path("resources");
00055   }
00056   else
00057   {
00058     resources_path = boost::filesystem::path(tmp);
00059   }
00060 
00061   std::string emcy_emergency_errors_filename = (resources_path / boost::filesystem::path("EMCY_schunk.ini")).string();
00062   EMCY::addEmergencyErrorMap( emcy_emergency_errors_filename, "schunk_error_codes");
00063 
00064 
00065 
00066   SchunkPowerBallNode::Ptr node_3 = my_controller.getNode<SchunkPowerBallNode> (3);
00067   SchunkPowerBallNode::Ptr node_4 = my_controller.getNode<SchunkPowerBallNode> (4);
00068   SchunkPowerBallNode::Ptr node_5 = my_controller.getNode<SchunkPowerBallNode> (5);
00069   SchunkPowerBallNode::Ptr node_6 = my_controller.getNode<SchunkPowerBallNode> (6);
00070   SchunkPowerBallNode::Ptr node_7 = my_controller.getNode<SchunkPowerBallNode> (7);
00071   SchunkPowerBallNode::Ptr node_8 = my_controller.getNode<SchunkPowerBallNode> (8);
00072 
00073 
00074   DS402Group::Ptr arm_group = my_controller.getGroup<DS402Group>("arm");
00075 
00076   SchunkPowerBallNode::Ptr node = node_7;
00077 //   SchunkPowerBallNode::Ptr node;
00078   if (node)
00079   {
00080     node->m_nmt.preOperational();
00081     node->printPDOMapping();
00082 //     PDO::MappingConfigurationList rpdo_mappings;
00083 //     // Map control and status word to the first PDO (receive and transmit respectively)
00084 //     rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
00085 //     rpdo_mappings.push_back(PDO::MappingConfiguration(0x607a, 0, 32, "target_position"));
00086 //
00087 //     node->appendPDOMappingSingle(rpdo_mappings, 1, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO, false);
00088     node->initNode();
00089     node_8->initNode();
00090     node->printSupportedModesOfOperation();
00091 
00092     node->setModeOfOperation(ds402::MOO_PROFILE_POSITION_MODE);
00093 
00094 //     ds402::ProfilePositionModeConfiguration config;
00095 //     config.profile_acceleration = 0.2;
00096 //     config.profile_velocity = 0.2;
00097 //     config.use_relative_targets = false;
00098 //     config.change_set_immediately = true;
00099 //     config.use_blending = true;
00100 //     config.use_relative_targets = false;
00101 //
00102 //     node_8->setupProfilePositionMode(config);
00103 
00104 
00105 //     uint32_t data32;
00106 //     node->m_sdo.upload(false, 0x1600, 0x01, data32);
00107 //     LOGGING_INFO(CanOpen, "RPDO-mapping 1: " << hexToString(data32) << endl);
00108 //     node->m_sdo.upload(false, 0x1600, 0x02, data32);
00109 //     LOGGING_INFO(CanOpen, "RPDO-mapping 2: " << hexToString(data32) << endl);
00110 //     node->m_sdo.upload(false, 0x1600, 0x03, data32);
00111 //     LOGGING_INFO(CanOpen, "RPDO-mapping 3: " << hexToString(data32) << endl);
00112 //
00113 //     node->m_sdo.upload(false, 0x1a00, 0x01, data32);
00114 //     LOGGING_INFO(CanOpen, "TPDO-mapping 1: " << hexToString(data32) << endl);
00115 //     node->m_sdo.upload(false, 0x1a00, 0x02, data32);
00116 //     LOGGING_INFO(CanOpen, "TPDO-mapping 2: " << hexToString(data32) << endl);
00117 //     node->m_sdo.upload(false, 0x1a00, 0x03, data32);
00118 //     LOGGING_INFO(CanOpen, "TPDO-mapping 3: " << hexToString(data32) << endl);
00119 //     node->configureHomingMethod(33);
00120   }
00121   else
00122   {
00123     arm_group->initNodes();
00124   }
00125 
00126 
00127 //   sleep (1);
00128   std::cout << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
00129 
00130   LOGGING_DEBUG (CanOpen, "Running syncAll once" << endl);
00131   my_controller.syncAll();
00132 
00133 
00134 //   sleep(3);
00135 
00136 //   node_8->printStatus();
00137 
00138   try
00139   {
00140     if (node)
00141     {
00142 //       node->initDS402State(ds402::STATE_OPERATION_ENABLE);
00143 //       node->home();
00144     }
00145     else
00146     {
00147       node_8->commutationSearch();
00148       std::cout << std::endl << std::endl << std::endl << std::endl << std::endl;
00149       sleep(2);
00150       node_7->commutationSearch();
00151       sleep(2);
00152       node_6->commutationSearch();
00153       sleep(2);
00154       node_5->commutationSearch();
00155       sleep(2);
00156       node_4->commutationSearch();
00157       sleep(2);
00158       node_3->commutationSearch();
00159 //       arm_group->initDS402State(ds402::STATE_OPERATION_ENABLE);
00160 //       std::vector<DS301Node::Ptr> nodes = arm_group->
00161     }
00162   }
00163   catch (const std::exception& e)
00164   {
00165     LOGGING_ERROR (CanOpen, e.what() << endl);
00166     return -1;
00167   }
00168 
00169   // check for recorded errors
00170 //   node_3->m_emcy->printLastErrors(node_3->m_sdo);
00171 //   node_3->m_emcy->clearErrorHistory(node_3->m_sdo);
00172 //   node_3->m_emcy->printLastErrors(node_3->m_sdo);
00173 
00174 
00175   std::cout << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
00176 
00177 //   return 0;
00178 
00179   size_t counter = 0;
00180   double target_position = 0.05;
00181   double current_position = 0;
00182 
00184 //   while (true)
00185 //   {
00186 //     try {
00187 //       my_controller.syncAll();
00188 //     }
00189 //     catch (const std::exception& e)
00190 //     {
00191 //       LOGGING_ERROR (CanOpen, e.what() << endl);
00192 //       return  -1;
00193 //     }
00194 //
00195 //     usleep(10000);
00196 //
00197 //     int state_int;
00198 //     std::cout << "In which state should I go? ";
00199 //     std::cin >> state_int;
00200 //     std::cout << std::endl;
00201 //
00202 //     ds402::eState state = static_cast<ds402::eState>(state_int);
00203 //
00204 //     node->initDS402State(state);
00205 //
00206 //     node->printStatus();
00207 //
00208 //     ++counter;
00209 //   }
00210 
00212 //   node->home();
00213 
00214   node_8->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
00215   while (counter < 5000)
00216   {
00217     try {
00218       my_controller.syncAll();
00219     }
00220     catch (const std::exception& e)
00221     {
00222       LOGGING_ERROR (CanOpen, e.what() << endl);
00223       return  -1;
00224     }
00225 
00226     usleep(10000);
00227 
00228     if (counter > 2)
00229     {
00230       if (node)
00231       {
00232         node->printStatus();
00233       }
00234       else
00235       {
00236         arm_group->printStatus();
00237       }
00238     }
00239 
00240     if (counter == 5)
00241     {
00242       try
00243       {
00244         node->enableNode(ds402::MOO_INTERPOLATED_POSITION_MODE);
00245         current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
00246         LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
00247       }
00248       catch (const std::exception& e)
00249       {
00250         LOGGING_ERROR (CanOpen, e.what() << endl);
00251         return -1;
00252       }
00253     }
00254 
00255     if (counter == 50 )
00256     {
00257       node_8->setTarget(1.57);
00258     }
00259 
00260     if (counter == 500 )
00261     {
00262       node_8->setTarget(-1.57);
00263     }
00264 
00265     if (counter == 1000 )
00266     {
00267       node_8->setTarget(1.57);
00268     }
00269     if (counter == 2000 )
00270     {
00271       node_8->setTarget(-1.57);
00272     }
00273 
00274     if (counter > 5 && counter < 299)
00275     {
00276       double starting_point = asin(current_position);
00277       target_position = sin(0.01*(counter-6) + starting_point);
00278       LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
00279 //       return 0;
00280       node->setTarget(target_position);
00281     }
00282 
00283 //     if (counter == 24)
00284 //     {
00285 //       try
00286 //       {
00287 // //         uint32_t vel = 1;
00288 // //         node->m_sdo.download(false, 0x60FF, 0, vel);
00289 // //         node->setTarget(vel);
00290 // //         node_8->resetFault();
00291 // //         node->setTarget(13);
00292 //         int32_t data32;
00293 //         uint32_t udata32;
00294 //
00295 //         // Buffer size
00296 //         node->m_sdo.upload(false, 0x60c4, 2, udata32);
00297 //         LOGGING_INFO(CanOpen, "Number of buffer entries: " << static_cast<int>(udata32) << endl);
00298 //
00299 //         // interpolation data record
00300 //         for (size_t i=1; i <= udata32; ++i)
00301 //         {
00302 //           node->m_sdo.upload(false, 0x60c1, i, data32);
00303 //           LOGGING_INFO(CanOpen, "Interpolation data record " << static_cast<int>(i) << ": " << static_cast<int>(data32) << endl);
00304 //         }
00305 //       }
00306 //       catch (const std::exception& e)
00307 //       {
00308 //         LOGGING_ERROR (CanOpen, e.what() << endl);
00309 //         return -1;
00310 //       }
00311 //     }
00312     if (counter == 300)
00313     {
00314       node->quickStop();
00315       LOGGING_INFO (CanOpen, "QuickStopped" << endl);
00316     }
00317 
00318     if (counter == 600)
00319     {
00320       current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
00321       LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
00322       node->enableNode(ds402::MOO_INTERPOLATED_POSITION_MODE);
00323     }
00324 
00325     if (counter >= 605 && counter < 800 )
00326     {
00327       double starting_point = asin(current_position);
00328       target_position = sin(0.01*(counter-605) + starting_point);
00329       LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
00330       node->setTarget(target_position);
00331 //       return 0;
00332     }
00333 
00334     if (counter == 600)
00335     {
00336       try
00337       {
00338 //         node_3->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
00339 //         node_4->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
00340 //         node_5->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
00341 //         node_6->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
00342 //         node_7->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
00343 //         node_8->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
00344       }
00345       catch (const std::exception& e)
00346       {
00347         LOGGING_ERROR (CanOpen, e.what() << endl);
00348         return -1;
00349       }
00350 
00351     }
00352 
00353     if (counter == 880)
00354     {
00355       node->closeBrakes();
00356     }
00357 
00358 
00359     if (counter == 1000)
00360     {
00361       current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
00362       LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
00363       node->openBrakes();
00364     }
00365 
00366     if (counter >= 1005 && counter < 2000)
00367     {
00368       LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
00369       double starting_point = asin(current_position);
00370       target_position = sin(0.01*(counter-1005) + starting_point);
00371       LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
00372       node->setTarget(target_position);
00373     }
00374 
00375     if (counter == 2000)
00376     {
00377       current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
00378       LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
00379       node->enableNode(ds402::MOO_INTERPOLATED_POSITION_MODE);
00380     }
00381 
00382     if (counter >= 2005)
00383     {
00384       double starting_point = asin(current_position);
00385       target_position = sin(0.01*(counter-2005) + starting_point);
00386       LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
00387       node->setTarget(target_position);
00388     }
00389 
00390     if (counter == 48)
00391     {
00392 //       node->disableNode();
00393     }
00394     ++counter;
00395   }
00396 
00398 
00399 
00400 //   ds402::Controlword word;
00401 //   word.all = 0x2f;
00402 //   target_position = 0.5;
00403 //
00404 //   // enable node
00405 //   node->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
00406 //   node_7->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
00407 //   // set target
00408 // //   node->m_sdo.download<uint32_t>(false, 0x607a, 0, 0);
00409 // //   sleep(1);
00410 // //   my_controller.syncAll();
00411 // //   node->printStatus();
00412 //
00413 // //   return 0;
00414 //   while (true)
00415 //   {
00416 //     try {
00417 //       my_controller.syncAll();
00418 //     }
00419 //     catch (const std::exception& e)
00420 //     {
00421 //       LOGGING_ERROR (CanOpen, e.what() << endl);
00422 //       return  -1;
00423 //     }
00424 //
00425 //     usleep(10000);
00426 //
00427 //     int16_t profile;
00428 //     node->printStatus();
00429 //     node->m_sdo.upload<int16_t>(false, 0x6086, 0, profile);
00430 // //     LOGGING_INFO (CanOpen, "Motion type profile is " << profile << endl);
00431 //
00432 //     if (counter == 5 )
00433 //     {
00434 //       LOGGING_INFO (CanOpen, "Publishing target position to device: " << target_position << endl);
00435 //       node->setTarget(target_position);
00436 //       node_7->setTarget(-target_position);
00437 //     }
00438 //
00439 //
00440 //     if(counter == 100)
00441 //     {
00442 //       target_position = 2 * target_position;
00443 //       node->setTarget(target_position);
00444 //       node_7->setTarget(-target_position);
00445 //     }
00446 //
00447 //     if(counter == 400)
00448 //     {
00449 //       target_position = 0;
00450 //       node->setTarget(target_position);
00451 //       node_7->setTarget(target_position);
00452 //     }
00453 //
00454 //     LOGGING_INFO (CanOpen, "Counter: " << counter << endl);
00455 //
00456 //     ++counter;
00457 //   }
00458 
00459 
00460 
00461 
00463 
00464 //   while (true)
00465 //   {
00466 //     try {
00467 //       my_controller.syncAll();
00468 //     }
00469 //     catch (const std::exception& e)
00470 //     {
00471 //       LOGGING_ERROR (CanOpen, e.what() << endl);
00472 //       return  -1;
00473 //     }
00474 //
00475 //     usleep(10000);
00476 //   }
00477 
00478   node->disableNode();
00479   node_8->disableNode();
00480 
00481   return 0;
00482 }


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Thu Jun 6 2019 20:17:24