Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
motor_control_thread.cpp
1 
2 /***************************************************************************
3  * motor_control_thread.cpp - Katana direct motor encoder/value control thread
4  *
5  * Created: Sun Mar 13 14:44:24 2011
6  * Copyright 2011 Bahram Maleki-Fard
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "motor_control_thread.h"
24 #include "controller.h"
25 #include "exception.h"
26 
27 #include <cstdlib>
28 
29 /** @class KatanaMotorControlThread "goto_thread.h"
30  * Katana motor control thread.
31  * This thread moves a single motor to/by a given value in encoders or angle.
32  * @author Bahram Maleki-Fard
33  */
34 
35 /** Constructor.
36  * @param katana katana controller base class
37  * @param logger logger
38  * @param poll_interval_ms interval in ms between two checks if the
39  * final position has been reached
40  */
42  fawkes::Logger *logger,
43  unsigned int poll_interval_ms)
44  : KatanaMotionThread("KatanaMotorControlThread", katana, logger)
45 {
46  __poll_interval_usec = poll_interval_ms * 1000;
47 }
48 
49 
50 /** Set target encoder value
51  * @param nr number of motor
52  * @param value encoder value
53  * @param inc is value incremental? deault:false -> absolute
54  */
55 void
56 KatanaMotorControlThread::set_encoder(unsigned int nr, int value, bool inc)
57 {
58  __nr = nr;
59  __encoder = value;
60 
61  __is_encoder = true;
62  __is_inc = inc;
63 }
64 
65 /** Set target angle value
66  * @param nr number of motor
67  * @param value angle value
68  * @param inc is value incremental? deault:false -> absolute
69  */
70 void
71 KatanaMotorControlThread::set_angle(unsigned int nr, float value, bool inc)
72 {
73  __nr = nr;
74  __angle = value;
75 
76  __is_encoder = false;
77  __is_inc = inc;
78 }
79 
80 
81 void
83 {
84  try {
85  // non-blocking call to KNI
86  if( __is_encoder ) {
87  if( __is_inc )
88 
89  _katana->move_motor_by(__nr, __encoder);
90  else
91  _katana->move_motor_to(__nr, __encoder);
92  } else {
93  if( __is_inc )
94  _katana->move_motor_by(__nr, __angle);
95  else
96  _katana->move_motor_to(__nr, __angle);
97  }
99  _logger->log_warn("KatanaMotorControlThread", "Motor %u out of range. Ex%s", __nr, e.what());
100  _finished = true;
102  return;
103  } catch (fawkes::Exception &e) {
104  _logger->log_warn("KatanaMotorControlThread", "Moving motor %u failed (ignoring): %s", __nr, e.what());
105  _finished = true;
107  return;
108  }
109 
110  // check if final
111  bool final = false;
112  short num_errors = 0;
113  while ( !final ) {
114  usleep(__poll_interval_usec);
115  try {
118  } catch (fawkes::Exception &e) {
119  if (++num_errors <= 10) {
120  _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
121  continue;
122  } else {
123  _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting");
125  break;
126  }
127  }
128 
129  try {
130  final = _katana->final();
132  _logger->log_warn("KatanaMotorControlTrhead", e.what());
134  break;
135  }
136  }
137 
138  _logger->log_debug(name(), "Successfully moved motor %u", __nr);
139 
140  _finished = true;
141 }
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void move_motor_to(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor to encoder value.
virtual void once()
Execute an action exactly once.
At least one motor is out of range.
Definition: exception.h:40
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:50
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:54
Katana motion thread base class.
Definition: motion_thread.h:37
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
KatanaMotorControlThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Base class for exceptions in Fawkes.
Definition: exception.h:36
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void move_motor_by(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor by encoder value (i.e.
At least one motor crashed.
Definition: exception.h:45
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:52
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:56
virtual void set_angle(unsigned int nr, float value, bool inc=false)
Set target angle value.
virtual void set_encoder(unsigned int nr, int value, bool inc=false)
Set target encoder value.
virtual bool final()=0
Check if movement is final.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
Interface for logging.
Definition: logger.h:34