Fawkes API
Fawkes Development Version
Main Page
Related Pages
Modules
Namespaces
Classes
Files
File List
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
*/
41
KatanaMotorControlThread::KatanaMotorControlThread
(
fawkes::RefPtr<fawkes::KatanaController>
katana,
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
82
KatanaMotorControlThread::once
()
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
}
98
}
catch
(
fawkes::KatanaOutOfRangeException
&e) {
99
_logger
->
log_warn
(
"KatanaMotorControlThread"
,
"Motor %u out of range. Ex%s"
, __nr, e.
what
());
100
_finished
=
true
;
101
_error_code
=
fawkes::KatanaInterface::ERROR_UNSPECIFIC
;
102
return
;
103
}
catch
(
fawkes::Exception
&e) {
104
_logger
->
log_warn
(
"KatanaMotorControlThread"
,
"Moving motor %u failed (ignoring): %s"
, __nr, e.
what
());
105
_finished
=
true
;
106
_error_code
=
fawkes::KatanaInterface::ERROR_CMD_START_FAILED
;
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
{
116
_katana
->
read_sensor_data
();
117
_katana
->
read_motor_data
();
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"
);
124
_error_code
=
fawkes::KatanaInterface::ERROR_COMMUNICATION
;
125
break
;
126
}
127
}
128
129
try
{
130
final
=
_katana
->
final
();
131
}
catch
(
fawkes::KatanaMotorCrashedException
&e) {
132
_logger
->
log_warn
(
"KatanaMotorControlTrhead"
, e.
what
());
133
_error_code
=
fawkes::KatanaInterface::ERROR_MOTOR_CRASHED
;
134
break
;
135
}
136
}
137
138
_logger
->
log_debug
(
name
(),
"Successfully moved motor %u"
, __nr);
139
140
_finished
=
true
;
141
}
src
plugins
katana
motor_control_thread.cpp
Generated by
1.8.1.1