pg70_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/DS402Group.h>
00015 #include <icl_hardware_canopen/Logging.h>
00016 #include <icl_hardware_canopen/ds402.h>
00017 #include <icl_hardware_canopen/SchunkPowerBallNode.h>
00018 
00019 using namespace icl_hardware::canopen_schunk;
00020 int main (int argc, char* argv[])
00021 {
00022   // Initializing
00023   icl_core::logging::initialize(argc, argv);
00024 
00025   CanOpenController my_controller("auto");
00026 
00027   DS402Group::Ptr arm_group = my_controller.getGroup<DS402Group>();
00028   my_controller.addNode<DS402Node>(12);
00029 
00030   DS402Node::Ptr node = my_controller.getNode<DS402Node>(12);
00031 
00032   node->setDefaultPDOMapping(DS402Node::PDO_MAPPING_PROFILE_POSITION_MODE);
00033   my_controller.initNodes();
00034 
00035   node->printSupportedModesOfOperation();
00036 
00037   ds402::ProfilePositionModeConfiguration config;
00038   config.profile_acceleration = 1.0;
00039   config.profile_velocity = 0.5;
00040   config.use_relative_targets = false;
00041   config.change_set_immediately = true;
00042   config.use_blending = false;
00043 
00044 
00045   node->setTransmissionFactor(65000);
00046   node->setModeOfOperation(ds402::MOO_PROFILE_POSITION_MODE);
00047   node->setDefaultPDOMapping(DS402Node::PDO_MAPPING_PROFILE_POSITION_MODE);
00048 
00049   float current_position;
00050   float target_position = 0.2;
00051 
00052   my_controller.syncAll();
00053 
00054 
00055   node->setupProfilePositionMode(config);
00056   sleep(1);
00057   node->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
00058   sleep(1);
00059 
00060   std::string target_string = "0.0";
00061 
00062   while (target_string != "q")
00063   {
00064     LOGGING_INFO(CanOpen, "Please insert a new target position (between 0 and 1, insert q to abort: " << endl);
00065     std::cin >> target_string;
00066     std::cout << std::endl;
00067 
00068     try
00069     {
00070       target_position = boost::lexical_cast<float>(target_string);
00071     }
00072     catch(boost::bad_lexical_cast &)
00073     {
00074       if (target_string != "q")
00075       {
00076         LOGGING_ERROR (CanOpen, "Invalid input! Please insert a numeric target or q to exit." << endl);
00077       }
00078       continue;
00079     }
00080 
00081     node->setTarget(target_position);
00082 
00083     bool is_reached = false;
00084 
00085     size_t counter = 0;
00086 
00087     while ((!is_reached && counter > 5) || counter <= 5)
00088     {
00089       try {
00090         my_controller.syncAll();
00091       }
00092       catch (const std::exception& e)
00093       {
00094         LOGGING_ERROR (CanOpen, e.what() << endl);
00095         return  -1;
00096       }
00097 
00098       usleep(10000);
00099 
00100       if (counter > 2)
00101       {
00102         if (node)
00103         {
00104           try
00105           {
00106             node->printStatus();
00107             is_reached = node->isTargetReached();
00108             current_position = node->getTPDOValue<int32_t>("measured_position");
00109             LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
00110           }
00111           catch (const std::exception& e)
00112           {
00113             LOGGING_ERROR (CanOpen, e.what() << endl);
00114             return -1;
00115           }
00116         }
00117       }
00118 
00119       if (counter == 5)
00120       {
00121         my_controller.enablePPMotion();
00122       }
00123       ++counter;
00124     }
00125 
00126     LOGGING_INFO (CanOpen, "Target reached" << endl);
00127   }
00128 
00129   node->disableNode();
00130 
00131   return 0;
00132 }


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