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