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 using 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  */
69 float
70 QuadraticMotorInstruct::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  */
119 float
120 QuadraticMotorInstruct::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
fawkes::BaseMotorInstruct::rot_acc_
float rot_acc_
Rotation acceleration.
Definition: base_motor_instruct.h:68
fawkes::QuadraticMotorInstruct::QuadraticMotorInstruct
QuadraticMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
Definition: quadratic_motor_instruct.cpp:53
fawkes::BaseMotorInstruct::rot_dec_
float rot_dec_
Rotation deceleration.
Definition: base_motor_instruct.h:69
fawkes::BaseMotorInstruct::trans_acc_
float trans_acc_
Translation acceleration.
Definition: base_motor_instruct.h:66
fawkes::BaseMotorInstruct::logger_
Logger * logger_
The fawkes logger.
Definition: base_motor_instruct.h:63
fawkes
fawkes::QuadraticMotorInstruct::~QuadraticMotorInstruct
virtual ~QuadraticMotorInstruct()
Destructor.
Definition: quadratic_motor_instruct.cpp:64
fawkes::BaseMotorInstruct::trans_dec_
float trans_dec_
Translation deceleration.
Definition: base_motor_instruct.h:67
fawkes::sqr
double sqr(double x)
Fast square multiplication.
Definition: common.h:43
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0