Fawkes API Fawkes Development Version
biward_drive_mode.cpp
1
2/***************************************************************************
3 * biward_drive_mode.cpp - Implementation of drive-mode "forward + 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 "biward_drive_mode.h"
24
25#include "backward_drive_mode.h"
26#include "forward_drive_mode.h"
27
28namespace fawkes {
29
30/** @class BiwardDriveModule <plugins/colli/drive_modes/biward_drive_mode.h>
31 * This is the SlowBiward drive-module. It is inherited from the abstract drive mode
32 * and uses the other both modes. If the target is in front, it drives forward
33 * to the target, else it drives backward to the target.
34 */
35
36/** Constructor.
37 * @param forward The Forward drive module
38 * @param backward The Backward drive module
39 * @param logger The fawkes logger
40 * @param config The fawkes configuration
41 */
43 BackwardDriveModule *backward,
44 Logger * logger,
45 Configuration * config)
46: AbstractDriveMode(logger, config)
47{
48 logger_->log_debug("BiwardDriveModule", "(Constructor): Entering...");
50 mod_forward_ = forward;
51 mod_backward_ = backward;
52
53 count_forward_ = 1;
54
55 max_trans_ = config_->get_float("/plugins/colli/drive_mode/normal/max_trans");
56 max_rot_ = config_->get_float("/plugins/colli/drive_mode/normal/max_rot");
57
58 logger_->log_debug("BiwardDriveModule", "(Constructor): Exiting...");
59}
60
61/** Destruct your local values here.
62 */
64{
65 logger_->log_debug("BiwardDriveModule", "(Destructor): Entering...");
66 logger_->log_debug("BiwardDriveModule", "(Destructor): Exiting...");
67}
68
69/* ************************************************************************** */
70/* *********************** U P D A T E ************************* */
71/* ************************************************************************** */
72
73/** Calculate here your desired settings. What you desire is checked afterwards to the current
74 * settings of the physical boundaries, but take care also.
75 *
76 * How you do this is up to you, but be careful, our hardware is expensive!!!!
77 *
78 * Available are:
79 *
80 * target_ --> current target coordinates to drive to
81 * robot_ --> current robot coordinates
82 * robot_vel_ --> current Motor velocities
83 *
84 * local_target_ --> our local target found by the search component we want to reach
85 * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
86 *
87 * orient_at_target_ --> Do we have to orient ourself at the target?
88 * stop_at_target_ --> Do we have to stop really ON the target?
89 *
90 * Afterwards filled should be:
91 *
92 * proposed_ --> Desired translation and rotation speed
93 *
94 * Those values are questioned after an update() was called.
95 */
96void
98{
99 // Just to take care.
101
102 // Our drive mode (choose between forward and backward)
103 AbstractDriveMode *drive_mode = NULL;
104
105 // search the correct drive mode
106 float angle_to_target = atan2(local_target_.y, local_target_.x);
107
108 if (count_forward_ == 1 && fabs(angle_to_target) > M_PI_2 + 0.1)
109 count_forward_ = -1;
110
111 else if (count_forward_ == 1)
112 count_forward_ = 1;
113
114 else if (count_forward_ == -1 && fabs(angle_to_target) < M_PI_2 - 0.1)
115 count_forward_ = 1;
116
117 else if (count_forward_ == -1)
118 count_forward_ = -1;
119
120 else {
121 logger_->log_debug("BiwardDriveModule", "Undefined state");
122 count_forward_ = 0;
123 }
124
125 if (count_forward_ == 1)
126 drive_mode = mod_forward_;
127 else
128 drive_mode = mod_backward_;
129
130 // set the current info to the drive mode
137
138 // update the drive mode
139 drive_mode->update();
140
141 // get the values from the drive mode
142 proposed_.x = drive_mode->get_proposed_trans_x();
143 proposed_.rot = drive_mode->get_proposed_rot();
144}
145
146} // namespace fawkes
This is the base class which calculates drive modes.
bool stop_at_target_
flag if stopping on or after target
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
float max_trans_
The maximum translation speed.
float get_proposed_trans_x()
Returns the proposed x translation.
float get_proposed_rot()
Returns the proposed rotatio.
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
cart_coord_2d_t local_trajec_
local trajectory
field_pos_t target_
current target
field_pos_t robot_
current robot pos
colli_trans_rot_t robot_vel_
current robot velocity
float max_rot_
The maximum rotation speed.
Configuration * config_
The fawkes configuration.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
Logger * logger_
The fawkes logger.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
cart_coord_2d_t local_target_
local target
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
This is the SlowBackward drive-module, for slow backward only movements.
BiwardDriveModule(ForwardDriveModule *forward, BackwardDriveModule *backward, Logger *logger, Configuration *config)
Constructor.
void update()
Calculate here your desired settings.
~BiwardDriveModule()
Destruct your local values here.
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.
This is the Forward drive-module, for forward only movements.
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
@ AllowBackward
Moving allow backward constant.
Fawkes library namespace.
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
float y
y coordinate in meters
Definition: types.h:127
float ori
orientation
Definition: types.h:128
float x
x coordinate in meters
Definition: types.h:126