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