simple_example.cpp
Go to the documentation of this file.
1 // -- BEGIN LICENSE BLOCK ----------------------------------------------
2 // This file is part of the SCHUNK Canopen Driver suite.
3 //
4 // This program is free software licensed under the LGPL
5 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
6 // You can find a copy of this license in LICENSE folder in the top
7 // directory of the source code.
8 //
9 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
10 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
11 // -- END LICENSE BLOCK ------------------------------------------------
12 
13 #include <icl_hardware_canopen/CanOpenController.h>
14 #include <icl_hardware_canopen/DS301Group.h>
15 #include <icl_hardware_canopen/SchunkPowerBallNode.h>
16 #include <icl_hardware_canopen/DS402Group.h>
17 #include <icl_hardware_canopen/exceptions.h>
18 #include <icl_hardware_canopen/PDO.h>
19 
20 #include <icl_hardware_canopen/Logging.h>
21 #include "../ds402.h"
22 
23 #include <cstdlib> // getenv
24 #include <boost/filesystem.hpp>
25 
26 using namespace icl_hardware::canopen_schunk;
27 
28 int main (int argc, char* argv[])
29 {
30  // Initializing
31  icl_core::logging::initialize(argc, argv);
32 
33  CanOpenController my_controller("auto");
34 
35  my_controller.addGroup<DS402Group>("arm");
36  my_controller.addNode<SchunkPowerBallNode>(3, "arm");
37  my_controller.addNode<SchunkPowerBallNode>(4, "arm");
38  my_controller.addNode<SchunkPowerBallNode>(5, "arm");
39  my_controller.addNode<SchunkPowerBallNode>(6, "arm");
40  my_controller.addNode<SchunkPowerBallNode>(7, "arm");
41  my_controller.addNode<SchunkPowerBallNode>(8, "arm");
42 
43  std::vector <uint8_t> output_data;
44  sleep(5); // wait for 5 seconds
45 
46  boost::filesystem::path resources_path;
47  char const* tmp = std::getenv("CANOPEN_RESOURCE_PATH");
48  if (tmp == NULL)
49  {
50  LOGGING_WARNING_C(
51  CanOpen,
52  CanOpenController,
53  "The environment variable 'CANOPEN_RESOURCE_PATH' could not be read. Using relative path 'resources/'" << endl);
54  resources_path = boost::filesystem::path("resources");
55  }
56  else
57  {
58  resources_path = boost::filesystem::path(tmp);
59  }
60 
61  std::string emcy_emergency_errors_filename = (resources_path / boost::filesystem::path("EMCY_schunk.ini")).string();
62  EMCY::addEmergencyErrorMap( emcy_emergency_errors_filename, "schunk_error_codes");
63 
64 
65 
66  SchunkPowerBallNode::Ptr node_3 = my_controller.getNode<SchunkPowerBallNode> (3);
67  SchunkPowerBallNode::Ptr node_4 = my_controller.getNode<SchunkPowerBallNode> (4);
68  SchunkPowerBallNode::Ptr node_5 = my_controller.getNode<SchunkPowerBallNode> (5);
69  SchunkPowerBallNode::Ptr node_6 = my_controller.getNode<SchunkPowerBallNode> (6);
70  SchunkPowerBallNode::Ptr node_7 = my_controller.getNode<SchunkPowerBallNode> (7);
71  SchunkPowerBallNode::Ptr node_8 = my_controller.getNode<SchunkPowerBallNode> (8);
72 
73 
74  DS402Group::Ptr arm_group = my_controller.getGroup<DS402Group>("arm");
75 
76  SchunkPowerBallNode::Ptr node = node_7;
77 // SchunkPowerBallNode::Ptr node;
78  if (node)
79  {
80  node->m_nmt.preOperational();
81  node->printPDOMapping();
82 // PDO::MappingConfigurationList rpdo_mappings;
83 // // Map control and status word to the first PDO (receive and transmit respectively)
84 // rpdo_mappings.push_back(PDO::MappingConfiguration(0x6040, 0, 16, "control_word"));
85 // rpdo_mappings.push_back(PDO::MappingConfiguration(0x607a, 0, 32, "target_position"));
86 //
87 // node->appendPDOMappingSingle(rpdo_mappings, 1, PDO::SYNCHRONOUS_CYCLIC, DS301Node::RECEIVE_PDO, false);
88  node->initNode();
89  node_8->initNode();
90  node->printSupportedModesOfOperation();
91 
92  node->setModeOfOperation(ds402::MOO_PROFILE_POSITION_MODE);
93 
94 // ds402::ProfilePositionModeConfiguration config;
95 // config.profile_acceleration = 0.2;
96 // config.profile_velocity = 0.2;
97 // config.use_relative_targets = false;
98 // config.change_set_immediately = true;
99 // config.use_blending = true;
100 // config.use_relative_targets = false;
101 //
102 // node_8->setupProfilePositionMode(config);
103 
104 
105 // uint32_t data32;
106 // node->m_sdo.upload(false, 0x1600, 0x01, data32);
107 // LOGGING_INFO(CanOpen, "RPDO-mapping 1: " << hexToString(data32) << endl);
108 // node->m_sdo.upload(false, 0x1600, 0x02, data32);
109 // LOGGING_INFO(CanOpen, "RPDO-mapping 2: " << hexToString(data32) << endl);
110 // node->m_sdo.upload(false, 0x1600, 0x03, data32);
111 // LOGGING_INFO(CanOpen, "RPDO-mapping 3: " << hexToString(data32) << endl);
112 //
113 // node->m_sdo.upload(false, 0x1a00, 0x01, data32);
114 // LOGGING_INFO(CanOpen, "TPDO-mapping 1: " << hexToString(data32) << endl);
115 // node->m_sdo.upload(false, 0x1a00, 0x02, data32);
116 // LOGGING_INFO(CanOpen, "TPDO-mapping 2: " << hexToString(data32) << endl);
117 // node->m_sdo.upload(false, 0x1a00, 0x03, data32);
118 // LOGGING_INFO(CanOpen, "TPDO-mapping 3: " << hexToString(data32) << endl);
119 // node->configureHomingMethod(33);
120  }
121  else
122  {
123  arm_group->initNodes();
124  }
125 
126 
127 // sleep (1);
128  std::cout << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
129 
130  LOGGING_DEBUG (CanOpen, "Running syncAll once" << endl);
131  my_controller.syncAll();
132 
133 
134 // sleep(3);
135 
136 // node_8->printStatus();
137 
138  try
139  {
140  if (node)
141  {
142 // node->initDS402State(ds402::STATE_OPERATION_ENABLE);
143 // node->home();
144  }
145  else
146  {
147  node_8->commutationSearch();
148  std::cout << std::endl << std::endl << std::endl << std::endl << std::endl;
149  sleep(2);
150  node_7->commutationSearch();
151  sleep(2);
152  node_6->commutationSearch();
153  sleep(2);
154  node_5->commutationSearch();
155  sleep(2);
156  node_4->commutationSearch();
157  sleep(2);
158  node_3->commutationSearch();
159 // arm_group->initDS402State(ds402::STATE_OPERATION_ENABLE);
160 // std::vector<DS301Node::Ptr> nodes = arm_group->
161  }
162  }
163  catch (const std::exception& e)
164  {
165  LOGGING_ERROR (CanOpen, e.what() << endl);
166  return -1;
167  }
168 
169  // check for recorded errors
170 // node_3->m_emcy->printLastErrors(node_3->m_sdo);
171 // node_3->m_emcy->clearErrorHistory(node_3->m_sdo);
172 // node_3->m_emcy->printLastErrors(node_3->m_sdo);
173 
174 
175  std::cout << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl;
176 
177 // return 0;
178 
179  size_t counter = 0;
180  double target_position = 0.05;
181  double current_position = 0;
182 
184 // while (true)
185 // {
186 // try {
187 // my_controller.syncAll();
188 // }
189 // catch (const std::exception& e)
190 // {
191 // LOGGING_ERROR (CanOpen, e.what() << endl);
192 // return -1;
193 // }
194 //
195 // usleep(10000);
196 //
197 // int state_int;
198 // std::cout << "In which state should I go? ";
199 // std::cin >> state_int;
200 // std::cout << std::endl;
201 //
202 // ds402::eState state = static_cast<ds402::eState>(state_int);
203 //
204 // node->initDS402State(state);
205 //
206 // node->printStatus();
207 //
208 // ++counter;
209 // }
210 
212 // node->home();
213 
214  node_8->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
215  while (counter < 5000)
216  {
217  try {
218  my_controller.syncAll();
219  }
220  catch (const std::exception& e)
221  {
222  LOGGING_ERROR (CanOpen, e.what() << endl);
223  return -1;
224  }
225 
226  usleep(10000);
227 
228  if (counter > 2)
229  {
230  if (node)
231  {
232  node->printStatus();
233  }
234  else
235  {
236  arm_group->printStatus();
237  }
238  }
239 
240  if (counter == 5)
241  {
242  try
243  {
244  node->enableNode(ds402::MOO_INTERPOLATED_POSITION_MODE);
245  current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
246  LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
247  }
248  catch (const std::exception& e)
249  {
250  LOGGING_ERROR (CanOpen, e.what() << endl);
251  return -1;
252  }
253  }
254 
255  if (counter == 50 )
256  {
257  node_8->setTarget(1.57);
258  }
259 
260  if (counter == 500 )
261  {
262  node_8->setTarget(-1.57);
263  }
264 
265  if (counter == 1000 )
266  {
267  node_8->setTarget(1.57);
268  }
269  if (counter == 2000 )
270  {
271  node_8->setTarget(-1.57);
272  }
273 
274  if (counter > 5 && counter < 299)
275  {
276  double starting_point = asin(current_position);
277  target_position = sin(0.01*(counter-6) + starting_point);
278  LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
279 // return 0;
280  node->setTarget(target_position);
281  }
282 
283 // if (counter == 24)
284 // {
285 // try
286 // {
287 // // uint32_t vel = 1;
288 // // node->m_sdo.download(false, 0x60FF, 0, vel);
289 // // node->setTarget(vel);
290 // // node_8->resetFault();
291 // // node->setTarget(13);
292 // int32_t data32;
293 // uint32_t udata32;
294 //
295 // // Buffer size
296 // node->m_sdo.upload(false, 0x60c4, 2, udata32);
297 // LOGGING_INFO(CanOpen, "Number of buffer entries: " << static_cast<int>(udata32) << endl);
298 //
299 // // interpolation data record
300 // for (size_t i=1; i <= udata32; ++i)
301 // {
302 // node->m_sdo.upload(false, 0x60c1, i, data32);
303 // LOGGING_INFO(CanOpen, "Interpolation data record " << static_cast<int>(i) << ": " << static_cast<int>(data32) << endl);
304 // }
305 // }
306 // catch (const std::exception& e)
307 // {
308 // LOGGING_ERROR (CanOpen, e.what() << endl);
309 // return -1;
310 // }
311 // }
312  if (counter == 300)
313  {
314  node->quickStop();
315  LOGGING_INFO (CanOpen, "QuickStopped" << endl);
316  }
317 
318  if (counter == 600)
319  {
320  current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
321  LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
322  node->enableNode(ds402::MOO_INTERPOLATED_POSITION_MODE);
323  }
324 
325  if (counter >= 605 && counter < 800 )
326  {
327  double starting_point = asin(current_position);
328  target_position = sin(0.01*(counter-605) + starting_point);
329  LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
330  node->setTarget(target_position);
331 // return 0;
332  }
333 
334  if (counter == 600)
335  {
336  try
337  {
338 // node_3->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
339 // node_4->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
340 // node_5->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
341 // node_6->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
342 // node_7->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
343 // node_8->initDS402State (icl_hardware::canopen_schunk::ds402::STATE_SWITCHED_ON);
344  }
345  catch (const std::exception& e)
346  {
347  LOGGING_ERROR (CanOpen, e.what() << endl);
348  return -1;
349  }
350 
351  }
352 
353  if (counter == 880)
354  {
355  node->closeBrakes();
356  }
357 
358 
359  if (counter == 1000)
360  {
361  current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
362  LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
363  node->openBrakes();
364  }
365 
366  if (counter >= 1005 && counter < 2000)
367  {
368  LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
369  double starting_point = asin(current_position);
370  target_position = sin(0.01*(counter-1005) + starting_point);
371  LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
372  node->setTarget(target_position);
373  }
374 
375  if (counter == 2000)
376  {
377  current_position = static_cast<double>(node->getTPDOValue<int32_t>("measured_position")) / SchunkPowerBallNode::RAD_TO_STEPS_FACTOR;
378  LOGGING_INFO (CanOpen, "Current position: " << current_position << endl);
379  node->enableNode(ds402::MOO_INTERPOLATED_POSITION_MODE);
380  }
381 
382  if (counter >= 2005)
383  {
384  double starting_point = asin(current_position);
385  target_position = sin(0.01*(counter-2005) + starting_point);
386  LOGGING_INFO (CanOpen, "Sinus target: " << target_position << endl);
387  node->setTarget(target_position);
388  }
389 
390  if (counter == 48)
391  {
392 // node->disableNode();
393  }
394  ++counter;
395  }
396 
398 
399 
400 // ds402::Controlword word;
401 // word.all = 0x2f;
402 // target_position = 0.5;
403 //
404 // // enable node
405 // node->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
406 // node_7->enableNode(ds402::MOO_PROFILE_POSITION_MODE);
407 // // set target
408 // // node->m_sdo.download<uint32_t>(false, 0x607a, 0, 0);
409 // // sleep(1);
410 // // my_controller.syncAll();
411 // // node->printStatus();
412 //
413 // // return 0;
414 // while (true)
415 // {
416 // try {
417 // my_controller.syncAll();
418 // }
419 // catch (const std::exception& e)
420 // {
421 // LOGGING_ERROR (CanOpen, e.what() << endl);
422 // return -1;
423 // }
424 //
425 // usleep(10000);
426 //
427 // int16_t profile;
428 // node->printStatus();
429 // node->m_sdo.upload<int16_t>(false, 0x6086, 0, profile);
430 // // LOGGING_INFO (CanOpen, "Motion type profile is " << profile << endl);
431 //
432 // if (counter == 5 )
433 // {
434 // LOGGING_INFO (CanOpen, "Publishing target position to device: " << target_position << endl);
435 // node->setTarget(target_position);
436 // node_7->setTarget(-target_position);
437 // }
438 //
439 //
440 // if(counter == 100)
441 // {
442 // target_position = 2 * target_position;
443 // node->setTarget(target_position);
444 // node_7->setTarget(-target_position);
445 // }
446 //
447 // if(counter == 400)
448 // {
449 // target_position = 0;
450 // node->setTarget(target_position);
451 // node_7->setTarget(target_position);
452 // }
453 //
454 // LOGGING_INFO (CanOpen, "Counter: " << counter << endl);
455 //
456 // ++counter;
457 // }
458 
459 
460 
461 
463 
464 // while (true)
465 // {
466 // try {
467 // my_controller.syncAll();
468 // }
469 // catch (const std::exception& e)
470 // {
471 // LOGGING_ERROR (CanOpen, e.what() << endl);
472 // return -1;
473 // }
474 //
475 // usleep(10000);
476 // }
477 
478  node->disableNode();
479  node_8->disableNode();
480 
481  return 0;
482 }
int32_t
signed int int32_t
icl_core::logging::initialize
bool initialize(int &argc, char *argv[], bool remove_read_arguments)
icl_hardware::canopen_schunk::EMCY::addEmergencyErrorMap
static void addEmergencyErrorMap(const std::string &filename, const std::string &block_identifier)
Adds new information from an ini file to the emergency error map.
Definition: EMCY.cpp:213
LOGGING_INFO
#define LOGGING_INFO(streamname, arg)
icl_hardware::canopen_schunk
Definition: CanOpenController.cpp:38
LOGGING_DEBUG
#define LOGGING_DEBUG(streamname, arg)
icl_hardware::canopen_schunk::SchunkPowerBallNode
This class gives a device specific interface for Schunk Powerballs, as they need some "special" treat...
Definition: SchunkPowerBallNode.h:46
icl_hardware::canopen_schunk::ds402::MOO_INTERPOLATED_POSITION_MODE
Definition: ds402.h:51
icl_hardware::canopen_schunk::ds402::MOO_PROFILE_POSITION_MODE
Definition: ds402.h:45
icl_hardware::canopen_schunk::DS402Group
The DS402Group class is the base class for canOpen devices implementing the DS402 device protocol (mo...
Definition: DS402Group.h:45
LOGGING_WARNING_C
#define LOGGING_WARNING_C(streamname, classname, arg)
LOGGING_ERROR
#define LOGGING_ERROR(streamname, arg)
boost::shared_ptr
sleep
unsigned int sleep(unsigned int seconds)
icl_hardware::canopen_schunk::CanOpenController
The CanOpenController class is the main entry point for any calls to the canOpen System.
Definition: CanOpenController.h:65
icl_hardware::canopen_schunk::CanOpenController::getNode
boost::shared_ptr< NodeT > getNode(const uint8_t node_id)
Returns a shared pointer handle to a node.
Definition: CanOpenController.hpp:124
endl
ThreadStream & endl(ThreadStream &stream)
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
icl_hardware::canopen_schunk::CanOpenController::getGroup
boost::shared_ptr< GroupT > getGroup(const std::string &index="default")
Returns a shared pointer to the group with a given index if possible.
Definition: CanOpenController.hpp:56
icl_hardware::canopen_schunk::CanOpenController::addNode
void addNode(const uint8_t node_id, const std::string &group_name="default")
Adds a new node to a group. If the group is not found (e.g. it was not created before), nothing will be done.
Definition: CanOpenController.hpp:80
icl_hardware::canopen_schunk::CanOpenController::addGroup
void addGroup(const std::string &identifier)
Adds a new node group with a given identifier. The group&#39;s type is given as template parameter...
Definition: CanOpenController.hpp:34
main
int main(int argc, char *argv[])
Definition: simple_example.cpp:28
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
icl_hardware::canopen_schunk::CanOpenController::syncAll
void syncAll()
Downloads all the RPDOs, Sends a Sync and Uploads all the TPDOs.
Definition: CanOpenController.cpp:547
icl_hardware::canopen_schunk::SchunkPowerBallNode::RAD_TO_STEPS_FACTOR
static const double RAD_TO_STEPS_FACTOR
This factor will be used to convert RAD numbers into encoder ticks.
Definition: SchunkPowerBallNode.h:57
usleep
int usleep(unsigned long useconds)

schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Mon Jun 10 2019 15:07:49