Fawkes API Fawkes Development Version
linear_motor_instruct.cpp
1
2/***************************************************************************
3 * linear_motor_instruct.cpp - Motor instructor with linear approximation
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013 Bahram Maleki-Fard
8 * 2014 Tobias Neumann
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24#include "linear_motor_instruct.h"
25
26#include <utils/math/common.h>
27
28#include <string>
29
30namespace fawkes {
31
32using namespace std;
33
34/** @class LinearMotorInstruct <plugins/colli/drive_realization/linear_motor_instruct.h>
35 * This module is a class for validity checks of drive
36 * commands and sets those things with respect to the physical
37 * borders of the robot.
38 * For this purpose the two functions calculate_rotation and
39 * calculate_translation are implemented linear ;-)
40 */
41
42/** Constructor.
43 * @param motor The MotorInterface with all the motor information
44 * @param frequency The frequency of the colli (should become deprecated!)
45 * @param logger The fawkes logger
46 * @param config The fawkes configuration
47 */
49 float frequency,
50 Logger * logger,
51 Configuration * config)
52: BaseMotorInstruct(motor, frequency, logger, config)
53{
54 logger_->log_debug("LinearMotorInstruct", "(Constructor): Entering");
55 logger_->log_debug("LinearMotorInstruct", "(Constructor): Exiting");
56}
57
58/** Destructor. */
60{
61 logger_->log_debug("LinearMotorInstruct", "(Destructor): Entering");
62 logger_->log_debug("LinearMotorInstruct", "(Destructor): Exiting");
63}
64
65/** Implementation of Calculate Translation Function.
66 * These are dangerous! Take care while modifying. Only a minus sign too few
67 * or too much may result in non predictable motor behaviour!!!!
68 * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
69 *
70 * @param current The current translation of the robot
71 * @param desired The desired translation of the robot
72 * @param time_factor The time_factor (should become deprecated!)
73 * @return the new translation
74 */
75float
76LinearMotorInstruct::calculate_translation(float current, float desired, float time_factor)
77{
78 float exec_trans = 0.0;
79
80 if (desired < current) {
81 if (current > 0.0) {
82 // decrease forward speed
83 exec_trans = current - trans_dec_;
84 exec_trans = max(exec_trans, desired);
85
86 } else if (current < 0.0) {
87 // increase backward speed
88 exec_trans = current - trans_acc_;
89 exec_trans = max(exec_trans, desired);
90
91 } else {
92 // current == 0;
93 exec_trans = max(-trans_acc_, desired);
94 }
95
96 } else if (desired > current) {
97 if (current > 0.0) {
98 // increase forward speed
99 exec_trans = current + trans_acc_;
100 exec_trans = min(exec_trans, desired);
101
102 } else if (current < 0.0) {
103 // decrease backward speed
104 exec_trans = current + trans_dec_;
105 exec_trans = min(exec_trans, desired);
106
107 } else {
108 // current == 0
109 exec_trans = min(trans_acc_, desired);
110 }
111
112 } else {
113 // nothing to change!!!
114 exec_trans = desired;
115 }
116
117 return exec_trans * time_factor;
118}
119
120/** Implementation of Calculate Rotation Function.
121 * These are dangerous! Take care while modifying. Only a minus sign too few
122 * or too much may result in non predictable motor behaviour!!!!
123 * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
124 *
125 * @param current The current rotation of the robot
126 * @param desired The desired rotation of the robot
127 * @param time_factor The time_factor (should become deprecated!)
128 * @return the new rotation
129 */
130float
131LinearMotorInstruct::calculate_rotation(float current, float desired, float time_factor)
132{
133 float exec_rot = 0.0;
134
135 if (desired < current) {
136 if (current > 0.0) {
137 // decrease right rot
138 exec_rot = current - rot_dec_;
139 exec_rot = max(exec_rot, desired);
140
141 } else if (current < 0.0) {
142 // increase left rot
143 exec_rot = current - rot_acc_;
144 exec_rot = max(exec_rot, desired);
145
146 } else {
147 // current == 0;
148 exec_rot = max(-rot_acc_, desired);
149 }
150
151 } else if (desired > current) {
152 if (current > 0.0) {
153 // increase right rot
154 exec_rot = current + rot_acc_;
155 exec_rot = min(exec_rot, desired);
156
157 } else if (current < 0.0) {
158 // decrease left rot
159 exec_rot = current + rot_dec_;
160 exec_rot = min(exec_rot, desired);
161
162 } else {
163 // current == 0
164 exec_rot = min(rot_acc_, desired);
165 }
166
167 } else {
168 // nothing to change!!!
169 exec_rot = desired;
170 }
171
172 return exec_rot * time_factor;
173}
174
175} // namespace fawkes
The Basic of a Motorinstructor.
float trans_acc_
Translation acceleration.
float rot_acc_
Rotation acceleration.
float rot_dec_
Rotation deceleration.
Logger * logger_
The fawkes logger.
float trans_dec_
Translation deceleration.
Interface for configuration handling.
Definition: config.h:68
virtual ~LinearMotorInstruct()
Destructor.
LinearMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
MotorInterface Fawkes BlackBoard Interface.
Fawkes library namespace.