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 
30 namespace fawkes
31 {
32 #if 0 /* just to make Emacs auto-indent happy */
33 }
34 #endif
35 
36 using namespace std;
37 
38 /** @class LinearMotorInstruct <plugins/colli/drive_realization/linear_motor_instruct.h>
39  * This module is a class for validity checks of drive
40  * commands and sets those things with respect to the physical
41  * borders of the robot.
42  * For this purpose the two functions calculate_rotation and
43  * calculate_translation are implemented linear ;-)
44  */
45 
46 /** Constructor.
47  * @param motor The MotorInterface with all the motor information
48  * @param frequency The frequency of the colli (should become deprecated!)
49  * @param logger The fawkes logger
50  * @param config The fawkes configuration
51  */
53  float frequency,
54  Logger* logger,
55  Configuration* config )
56  : BaseMotorInstruct( motor, frequency, logger, config )
57 {
58  logger_->log_debug("LinearMotorInstruct", "(Constructor): Entering");
59  logger_->log_debug("LinearMotorInstruct", "(Constructor): Exiting");
60 }
61 
62 
63 /** Destructor. */
65 {
66  logger_->log_debug("LinearMotorInstruct", "(Destructor): Entering");
67  logger_->log_debug("LinearMotorInstruct", "(Destructor): Exiting");
68 }
69 
70 
71 /** Implementation of Calculate Translation Function.
72  * These are dangerous! Take care while modifying. Only a minus sign too few
73  * or too much may result in non predictable motor behaviour!!!!
74  * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
75  *
76  * @param current The current translation of the robot
77  * @param desired The desired translation of the robot
78  * @param time_factor The time_factor (should become deprecated!)
79  * @return the new translation
80  */
81 float LinearMotorInstruct::calculate_translation( float current, float desired, float time_factor )
82 {
83  float exec_trans = 0.0;
84 
85  if (desired < current) {
86 
87  if (current > 0.0) {
88  // decrease forward speed
89  exec_trans = current - trans_dec_;
90  exec_trans = max( exec_trans, desired );
91 
92  } else if (current < 0.0) {
93  // increase backward speed
94  exec_trans = current - trans_acc_;
95  exec_trans = max( exec_trans, desired );
96 
97  } else {
98  // current == 0;
99  exec_trans = max( -trans_acc_, desired );
100  }
101 
102  } else if (desired > current) {
103 
104  if (current > 0.0) {
105  // increase forward speed
106  exec_trans = current + trans_acc_;
107  exec_trans = min( exec_trans, desired );
108 
109  } else if (current < 0.0) {
110  // decrease backward speed
111  exec_trans = current + trans_dec_;
112  exec_trans = min( exec_trans, desired );
113 
114  } else {
115  // current == 0
116  exec_trans = min( trans_acc_, desired );
117  }
118 
119  } else {
120  // nothing to change!!!
121  exec_trans = desired;
122  }
123 
124  return exec_trans*time_factor;
125 }
126 
127 
128 /** Implementation of Calculate Rotation Function.
129  * These are dangerous! Take care while modifying. Only a minus sign too few
130  * or too much may result in non predictable motor behaviour!!!!
131  * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
132  *
133  * @param current The current rotation of the robot
134  * @param desired The desired rotation of the robot
135  * @param time_factor The time_factor (should become deprecated!)
136  * @return the new rotation
137  */
138 float LinearMotorInstruct::calculate_rotation( float current, float desired, float time_factor )
139 {
140  float exec_rot = 0.0;
141 
142  if (desired < current) {
143 
144  if (current > 0.0) {
145  // decrease right rot
146  exec_rot = current - rot_dec_;
147  exec_rot = max( exec_rot, desired );
148 
149  } else if (current < 0.0) {
150  // increase left rot
151  exec_rot = current - rot_acc_;
152  exec_rot = max( exec_rot, desired );
153 
154  } else {
155  // current == 0;
156  exec_rot = max( -rot_acc_, desired );
157  }
158 
159  } else if (desired > current) {
160  if (current > 0.0) {
161  // increase right rot
162  exec_rot = current + rot_acc_;
163  exec_rot = min( exec_rot, desired );
164 
165  } else if (current < 0.0) {
166  // decrease left rot
167  exec_rot = current + rot_dec_;
168  exec_rot = min( exec_rot, desired );
169 
170  } else {
171  // current == 0
172  exec_rot = min( rot_acc_, desired );
173  }
174 
175  } else {
176  // nothing to change!!!
177  exec_rot = desired;
178  }
179 
180  return exec_rot*time_factor;
181 }
182 
183 } // namespace fawkes
Fawkes library namespace.
STL namespace.
float trans_dec_
Translation deceleration.
float rot_acc_
Rotation acceleration.
float rot_dec_
Rotation deceleration.
Logger * logger_
The fawkes logger.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float trans_acc_
Translation acceleration.
LinearMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
The Basic of a Motorinstructor.
MotorInterface Fawkes BlackBoard Interface.
virtual ~LinearMotorInstruct()
Destructor.
Interface for configuration handling.
Definition: config.h:67
Interface for logging.
Definition: logger.h:34