Fawkes API  Fawkes Development Version
quadratic_motor_instruct.cpp
1 
2 /***************************************************************************
3  * quadratic_motor_instruct.cpp - Motor instructor with quadratic approximation
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013 Bahram Maleki-Fard
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 "quadratic_motor_instruct.h"
24 
25 #include <utils/math/common.h>
26 
27 #include <string>
28 
29 namespace fawkes
30 {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 using namespace std;
36 
37 /** @class QuadraticMotorInstruct <plugins/colli/drive_realization/quadratic_motor_instruct.h>
38  * This module is a class for validity checks of drive
39  * commands and sets those things with respect to the physical
40  * borders of the robot.
41  * For this purpose the two functions CalculateRotation and
42  * CalculateTranslation are implemented quadratically ;-)
43  */
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("QuadraticMotorInstruct", "(Constructor): Entering");
59  logger_->log_debug("QuadraticMotorInstruct", "(Constructor): Exiting");
60 }
61 
62 /** Destructor. */
64 {
65  logger_->log_debug("QuadraticMotorInstruct", "(Destructor): Entering");
66  logger_->log_debug("QuadraticMotorInstruct", "(Destructor): Exiting");
67 }
68 
69 
70 /** Implementation of Calculate Translation Function.
71  * These are dangerous! Take care while modifying. Only a minus sign too few
72  * or too much may result in non predictable motor behaviour!!!!
73  * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
74  */
75 float
76 QuadraticMotorInstruct::calculate_translation( float current, float desired, float time_factor )
77 {
78  float exec_trans = 0.0;
79 
80  if (desired < current) {
81 
82  if (current > 0.0) {
83  // decrease forward speed
84  exec_trans = current - trans_dec_ - ((sqr( fabs(current) + 1.0 ) * trans_dec_) / 8.0);
85  exec_trans = max( exec_trans, desired );
86 
87  } else if (current < 0.0) {
88  // increase backward speed
89  exec_trans = current - trans_acc_ - ((sqr( fabs(current) + 1.0 ) * trans_acc_) / 8.0);
90  exec_trans = max( exec_trans, desired );
91 
92  } else {
93  // current == 0;
94  exec_trans = max( -trans_acc_, desired );
95  }
96 
97  } else if (desired > current) {
98 
99  if (current > 0.0) {
100  // increase forward speed
101  exec_trans = current + trans_acc_ + ((sqr( fabs(current) + 1.0 ) * trans_acc_) / 8.0);
102  exec_trans = min( exec_trans, desired );
103 
104  } else if (current < 0.0) {
105  // decrease backward speed
106  exec_trans = current + trans_dec_ + ((sqr( fabs(current) + 1.0 ) * trans_dec_) / 8.0);
107  exec_trans = min( exec_trans, desired );
108 
109  } else {
110  // current == 0
111  exec_trans = min( trans_acc_, desired );
112  }
113 
114  } else {
115  // nothing to change!!!
116  exec_trans = desired;
117  }
118 
119  return exec_trans*time_factor;
120 }
121 
122 
123 /** Implementation of Calculate Rotation Function.
124  * These are dangerous! Take care while modifying. Only a minus sign too few
125  * or too much may result in non predictable motor behaviour!!!!
126  * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
127  */
128 float QuadraticMotorInstruct::calculate_rotation( float current, float desired, float time_factor )
129 {
130  float exec_rot = 0.0;
131 
132  if (desired < current) {
133 
134  if (current > 0.0) {
135  // decrease right rot
136  exec_rot = current - rot_dec_ - ((sqr( fabs(current) + 1.0 ) * rot_dec_) / 8.0);
137  exec_rot = max( exec_rot, desired );
138 
139  } else if (current < 0.0) {
140  // increase left rot
141  exec_rot = current - rot_acc_ - ((sqr( fabs(current) + 1.0 ) * rot_acc_) / 8.0);
142  exec_rot = max( exec_rot, desired );
143 
144  } else {
145  // current == 0;
146  exec_rot = max( -rot_acc_, desired );
147  }
148 
149  } else if (desired > current) {
150  if (current > 0.0) {
151  // increase right rot
152  exec_rot = current + rot_acc_ + ((sqr( fabs(current) + 1.0 ) * rot_acc_) / 8.0);
153  exec_rot = min( exec_rot, desired );
154 
155  } else if (current < 0.0) {
156  // decrease left rot
157  exec_rot = current + rot_dec_ + ((sqr( fabs(current) + 1.0 ) * rot_dec_) / 8.0);
158  exec_rot = min( exec_rot, desired );
159 
160  } else {
161  // current == 0
162  exec_rot = min( rot_acc_, desired );
163  }
164 
165  } else {
166  // nothing to change!!!
167  exec_rot = desired;
168  }
169 
170  return exec_rot*time_factor;
171 }
172 
173 } // namespace fawkes
Fawkes library namespace.
QuadraticMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
STL namespace.
float trans_dec_
Translation deceleration.
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
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.
The Basic of a Motorinstructor.
MotorInterface Fawkes BlackBoard Interface.
Interface for configuration handling.
Definition: config.h:67
Interface for logging.
Definition: logger.h:34