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
31namespace fawkes {
32
33using 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 */
76float
77EmergencyMotorInstruct::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 */
129float
130EmergencyMotorInstruct::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
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.
Interface for configuration handling.
Definition: config.h:68
EmergencyMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
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.
Fawkes library namespace.