Fawkes API Fawkes Development Version
backward_drive_mode.cpp
1
2/***************************************************************************
3 * backward_drive_mode.cpp - Implementation of drive-mode "backward"
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013-2014 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 "backward_drive_mode.h"
24
25#include <utils/math/angle.h>
26#include <utils/math/common.h>
27
28namespace fawkes {
29
30/** @class BackwardDriveModule <plugins/colli/drive_modes/backward_drive_mode.h>
31 * This is the SlowBackward drive-module, for slow backward only movements.
32 */
33
34/** Constructor.
35 * @param logger The fawkes logger
36 * @param config The fawkes configuration
37 */
39: AbstractDriveMode(logger, config)
40{
41 logger_->log_debug("BackwardDriveModule", "(Constructor): Entering...");
43
44 max_trans_ = config_->get_float("/plugins/colli/drive_mode/normal/max_trans");
45 max_rot_ = config_->get_float("/plugins/colli/drive_mode/normal/max_rot");
46
47 logger_->log_debug("BackwardDriveModule", "(Constructor): Exiting");
48}
49
50/** Destruct your local values here!
51 */
53{
54 logger_->log_debug("BackwardDriveModule", "(Destructor): Entering...");
56 logger_->log_debug("BackwardDriveModule", "(Destructor): Exiting");
57}
58
59/** Calculate by given variables a new rotation to give for the motor to minimize curvature.
60 *
61 * DOC.: This here is the most complicated part in realizing the colli. By the given information
62 * I have to calculate a rotation I want to achieve in an optimal way.
63 * Here this is solved in an interesting way:
64 * First, check how long the curvature is, we want to drive to the target. This is done by
65 * approximating the size of the triangle around this curvature given by collision and
66 * and targetpoint and the angle between those both. Afterwards the time we have to drive
67 * with constant speed is calculated. Now we have the time we want to turn the angle. By
68 * multiplying this with a high constant (here 4), we rotate faster than we want to, but
69 * so the curvatures are not so extraordinary big. Afterwards there is an proportional
70 * part added, so we have a control factor here. (P-Regler ;-) )
71 *
72 * @return A desired rotation.
73 */
74float
75BackwardDriveModule::backward_curvature(float dist_to_target,
76 float dist_to_trajec,
77 float alpha,
78 float cur_trans,
79 float cur_rot)
80{
81 return 1.2f * alpha;
82}
83
84/** Calculate by given variables a new translation to give for the motor to
85 * minimize distance to the target.
86 *
87 * DOC.: This here is a fairly easy routine after the previous one ;-). It calculates
88 * to a proposed rotation the translation part. This means, in relation to
89 * distances to target and trajec and the current values we calculate a new one.
90 *
91 * @return A desired translation.
92 */
93float
94BackwardDriveModule::backward_translation(float dist_to_target,
95 float dist_to_front,
96 float alpha,
97 float cur_trans,
98 float cur_rot,
99 float des_rot)
100{
101 float des_trans = 0.f;
102
103 if (fabs(des_rot) >= 0.f && fabs(des_rot) <= 1.f)
104 des_trans = lin_interpol(fabs(des_rot), 0.f, 1.f, 0.7f, fabs(max_trans_ + 0.1f));
105
106 else if (fabs(des_rot) > 1.f)
107 des_trans = lin_interpol(fabs(des_rot), M_PI, 1.f, 0.f, 0.7f);
108
109 // test the borders (no agressive behaviour!)
110 if (des_trans > 0.f)
111 des_trans = 0.f;
112 if (des_trans < max_trans_)
113 des_trans = max_trans_;
114
115 // OLD STUFF
116 // // check stopping on target and compare distances with choosen velocities
117 // if ( fabs( dist_to_target - dist_to_front ) < 0.2 )
118 // {
119 // if (stop_at_target_ == true)
120 // des_trans = min( des_trans, dist_to_target*1.5 );
121 // else
122 // ; // do not stop, so drive behind the target with full power
123 // }
124 // else
125 // {
126 // des_trans = min( des_trans, dist_to_front );
127 // }
128
129 // NEW STUFF
130 float trans_target = 10000.f;
131 float trans_front = 10000.f;
132
133 if (stop_at_target_ == true)
134 trans_target = guarantee_trans_stop(dist_to_target, cur_trans, des_trans);
135
136 // And the next collision point
137 if (dist_to_front < dist_to_target)
138 trans_front = guarantee_trans_stop((1.f * dist_to_front) / 2.f, cur_trans, des_trans);
139 // NEW STUFF END HERE
140
141 des_trans = std::min(des_trans, std::min(trans_target, trans_front));
142
143 return des_trans;
144}
145
146/* ************************************************************************** */
147/* *********************** U P D A T E ************************* */
148/* ************************************************************************** */
149
150/** Calculate here your desired settings. What you desire is checked afterwards to the current
151 * settings of the physical boundaries, but take care also.
152 *
153 * How you do this is up to you, but be careful, our hardware is expensive!!!!
154 *
155 * Available are:
156 *
157 * target_ --> current target coordinates to drive to
158 * robot_ --> current robot coordinates
159 * robot_vel_ --> current Motor velocities
160 *
161 * local_target_ --> our local target found by the search component we want to reach
162 * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
163 *
164 * orient_at_target_ --> Do we have to orient ourself at the target?
165 * stop_at_target_ --> Do we have to stop really ON the target?
166 *
167 * Afterwards filled should be:
168 *
169 * proposed_ --> Desired translation and rotation speed
170 *
171 * Those values are questioned after an update() was called.
172 */
173void
175{
177
178 float dist_to_target = sqrt(sqr(local_target_.x) + sqr(local_target_.y));
179 float alpha = normalize_mirror_rad(atan2(local_target_.y, local_target_.x) + M_PI);
180 float dist_to_trajec = sqrt(sqr(local_trajec_.x) + sqr(local_trajec_.y));
181
182 proposed_.rot =
183 backward_curvature(dist_to_target, dist_to_trajec, alpha, -robot_speed_, -robot_vel_.rot);
184
185 if (fabs(alpha) <= M_PI_2 + 0.1f)
186 proposed_.x = backward_translation(
187 dist_to_target, dist_to_trajec, alpha, -robot_speed_, -robot_vel_.rot, proposed_.rot);
188
189 // last time border check............. IMPORTANT!!!
190 // because the motorinstructor just tests robots physical borders.
191 if (dist_to_target >= 0.04f) {
192 proposed_.x = std::min(proposed_.x, max_trans_);
193 proposed_.x = std::max(proposed_.x, 0.f);
194 proposed_.x *= -1.f;
195
196 if (proposed_.rot > max_rot_)
198
199 if (proposed_.rot < -max_rot_)
201
202 if (!stop_at_target_ && dist_to_target < 1.f) {
203 // reduce rotation velocity to avoid wild rotations
204 if (proposed_.rot > 0.5f)
205 proposed_.rot = 0.5f;
206 else if (proposed_.rot < -0.5f)
207 proposed_.rot = -0.5f;
208 }
209 }
210}
211
212} // namespace fawkes
This is the base class which calculates drive modes.
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
cart_coord_2d_t local_trajec_
local trajectory
float robot_speed_
current robo translation velocity
colli_trans_rot_t robot_vel_
current robot velocity
float max_rot_
The maximum rotation speed.
Configuration * config_
The fawkes configuration.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
Logger * logger_
The fawkes logger.
cart_coord_2d_t local_target_
local target
void update()
Calculate here your desired settings.
~BackwardDriveModule()
Destruct your local values here!
BackwardDriveModule(Logger *logger, Configuration *config)
Constructor.
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
@ Backward
Moving backward constant.
@ MovingNotAllowed
Moving not allowed constant.
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66
float x
Translation in x-direction.
Definition: types.h:61
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63