Fawkes API Fawkes Development Version
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-2014 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
25#include "controller.h"
26#include "exception.h"
27
28#include <interfaces/KatanaInterface.h>
29
30#include <cstdlib>
31#include <unistd.h>
32
33/** @class KatanaMotorControlThread "goto_thread.h"
34 * Katana motor control thread.
35 * This thread moves a single motor to/by a given value in encoders or angle.
36 * @author Bahram Maleki-Fard
37 */
38
39/** Constructor.
40 * @param katana katana controller base class
41 * @param logger logger
42 * @param poll_interval_ms interval in ms between two checks if the
43 * final position has been reached
44 */
46 fawkes::Logger * logger,
47 unsigned int poll_interval_ms)
48: KatanaMotionThread("KatanaMotorControlThread", katana, logger)
49{
50 poll_interval_usec_ = poll_interval_ms * 1000;
51}
52
53/** Set target encoder value
54 * @param nr number of motor
55 * @param value encoder value
56 * @param inc is value incremental? deault:false -> absolute
57 */
58void
59KatanaMotorControlThread::set_encoder(unsigned int nr, int value, bool inc)
60{
61 nr_ = nr;
62 encoder_ = value;
63
64 is_encoder_ = true;
65 is_inc_ = inc;
66}
67
68/** Set target angle value
69 * @param nr number of motor
70 * @param value angle value
71 * @param inc is value incremental? deault:false -> absolute
72 */
73void
74KatanaMotorControlThread::set_angle(unsigned int nr, float value, bool inc)
75{
76 nr_ = nr;
77 angle_ = value;
78
79 is_encoder_ = false;
80 is_inc_ = inc;
81}
82
83void
85{
86 try {
87 // non-blocking call to KNI
88 if (is_encoder_) {
89 if (is_inc_)
90
91 _katana->move_motor_by(nr_, encoder_);
92 else
93 _katana->move_motor_to(nr_, encoder_);
94 } else {
95 if (is_inc_)
96 _katana->move_motor_by(nr_, angle_);
97 else
98 _katana->move_motor_to(nr_, angle_);
99 }
101 _logger->log_warn("KatanaMotorControlThread", "Motor %u out of range. Ex%s", nr_, e.what());
102 _finished = true;
104 return;
105 } catch (fawkes::Exception &e) {
106 _logger->log_warn("KatanaMotorControlThread",
107 "Moving motor %u failed (ignoring): %s",
108 nr_,
109 e.what());
110 _finished = true;
112 return;
113 }
114
115 // check if final
116 bool final = false;
117 short num_errors = 0;
118 while (!final) {
119 usleep(poll_interval_usec_);
120 try {
123 } catch (fawkes::Exception &e) {
124 if (++num_errors <= 10) {
125 _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
126 continue;
127 } else {
128 _logger->log_warn("KatanaMotorControlThread",
129 "Receiving sensor/motor data failed too often, aborting");
131 break;
132 }
133 }
134
135 try {
136 final = _katana->final();
138 _logger->log_warn("KatanaMotorControlThread", "Motor crashed: %s", e.what());
140 break;
141 }
142 }
143
144 _logger->log_debug(name(), "Successfully moved motor %u", nr_);
145
146 _finished = true;
147}
Katana motion thread base class.
Definition: motion_thread.h:36
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:52
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:48
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:50
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:54
KatanaMotorControlThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
virtual void once()
Execute an action exactly once.
virtual void set_encoder(unsigned int nr, int value, bool inc=false)
Set target encoder value.
virtual void set_angle(unsigned int nr, float value, bool inc=false)
Set target angle value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void read_sensor_data()=0
Read all sensor data 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 move_motor_by(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor by encoder value (i.e.
virtual bool final()=0
Check if movement is final.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
At least one motor crashed.
Definition: exception.h:44
At least one motor is out of range.
Definition: exception.h:38
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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:100