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
29namespace fawkes {
30
31using namespace std;
32
33/** @class QuadraticMotorInstruct <plugins/colli/drive_realization/quadratic_motor_instruct.h>
34 * This module is a class for validity checks of drive
35 * commands and sets those things with respect to the physical
36 * borders of the robot.
37 * For this purpose the two functions CalculateRotation and
38 * CalculateTranslation are implemented quadratically ;-)
39 */
40
41/** Constructor.
42 * @param motor The MotorInterface with all the motor information
43 * @param frequency The frequency of the colli (should become deprecated!)
44 * @param logger The fawkes logger
45 * @param config The fawkes configuration
46 */
48 float frequency,
49 Logger * logger,
50 Configuration * config)
51: BaseMotorInstruct(motor, frequency, logger, config)
52{
53 logger_->log_debug("QuadraticMotorInstruct", "(Constructor): Entering");
54 logger_->log_debug("QuadraticMotorInstruct", "(Constructor): Exiting");
55}
56
57/** Destructor. */
59{
60 logger_->log_debug("QuadraticMotorInstruct", "(Destructor): Entering");
61 logger_->log_debug("QuadraticMotorInstruct", "(Destructor): Exiting");
62}
63
64/** Implementation of Calculate Translation Function.
65 * These are dangerous! Take care while modifying. Only a minus sign too few
66 * or too much may result in non predictable motor behaviour!!!!
67 * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
68 */
69float
70QuadraticMotorInstruct::calculate_translation(float current, float desired, float time_factor)
71{
72 float exec_trans = 0.0;
73
74 if (desired < current) {
75 if (current > 0.0) {
76 // decrease forward speed
77 exec_trans = current - trans_dec_ - ((sqr(fabs(current) + 1.0) * trans_dec_) / 8.0);
78 exec_trans = max(exec_trans, desired);
79
80 } else if (current < 0.0) {
81 // increase backward speed
82 exec_trans = current - trans_acc_ - ((sqr(fabs(current) + 1.0) * trans_acc_) / 8.0);
83 exec_trans = max(exec_trans, desired);
84
85 } else {
86 // current == 0;
87 exec_trans = max(-trans_acc_, desired);
88 }
89
90 } else if (desired > current) {
91 if (current > 0.0) {
92 // increase forward speed
93 exec_trans = current + trans_acc_ + ((sqr(fabs(current) + 1.0) * trans_acc_) / 8.0);
94 exec_trans = min(exec_trans, desired);
95
96 } else if (current < 0.0) {
97 // decrease backward speed
98 exec_trans = current + trans_dec_ + ((sqr(fabs(current) + 1.0) * trans_dec_) / 8.0);
99 exec_trans = min(exec_trans, desired);
100
101 } else {
102 // current == 0
103 exec_trans = min(trans_acc_, desired);
104 }
105
106 } else {
107 // nothing to change!!!
108 exec_trans = desired;
109 }
110
111 return exec_trans * time_factor;
112}
113
114/** Implementation of Calculate Rotation Function.
115 * These are dangerous! Take care while modifying. Only a minus sign too few
116 * or too much may result in non predictable motor behaviour!!!!
117 * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
118 */
119float
120QuadraticMotorInstruct::calculate_rotation(float current, float desired, float time_factor)
121{
122 float exec_rot = 0.0;
123
124 if (desired < current) {
125 if (current > 0.0) {
126 // decrease right rot
127 exec_rot = current - rot_dec_ - ((sqr(fabs(current) + 1.0) * rot_dec_) / 8.0);
128 exec_rot = max(exec_rot, desired);
129
130 } else if (current < 0.0) {
131 // increase left rot
132 exec_rot = current - rot_acc_ - ((sqr(fabs(current) + 1.0) * rot_acc_) / 8.0);
133 exec_rot = max(exec_rot, desired);
134
135 } else {
136 // current == 0;
137 exec_rot = max(-rot_acc_, desired);
138 }
139
140 } else if (desired > current) {
141 if (current > 0.0) {
142 // increase right rot
143 exec_rot = current + rot_acc_ + ((sqr(fabs(current) + 1.0) * rot_acc_) / 8.0);
144 exec_rot = min(exec_rot, desired);
145
146 } else if (current < 0.0) {
147 // decrease left rot
148 exec_rot = current + rot_dec_ + ((sqr(fabs(current) + 1.0) * rot_dec_) / 8.0);
149 exec_rot = min(exec_rot, desired);
150
151 } else {
152 // current == 0
153 exec_rot = min(rot_acc_, desired);
154 }
155
156 } else {
157 // nothing to change!!!
158 exec_rot = desired;
159 }
160
161 return exec_rot * time_factor;
162}
163
164} // 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
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.
QuadraticMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
Definition: common.h:37