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 
28 namespace 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  */
42 BiwardDriveModule::BiwardDriveModule(ForwardDriveModule * forward,
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  */
96 void
98 {
99  // Just to take care.
100  proposed_.x = proposed_.y = proposed_.rot = 0.f;
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
131  drive_mode->set_current_robo_pos(robot_.x, robot_.y, robot_.ori);
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
fawkes::BiwardDriveModule::update
void update()
Calculate here your desired settings.
Definition: biward_drive_mode.cpp:102
fawkes::AbstractDriveMode::set_current_robo_speed
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
Definition: abstract_drive_mode.h:227
fawkes::NavigatorInterface::AllowBackward
Moving allow backward constant.
Definition: NavigatorInterface.h:66
fawkes::AbstractDriveMode::set_local_trajec
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
Definition: abstract_drive_mode.h:267
fawkes::AbstractDriveMode::robot_
field_pos_t robot_
current robot pos
Definition: abstract_drive_mode.h:91
fawkes::AbstractDriveMode::proposed_
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
Definition: abstract_drive_mode.h:102
fawkes::colli_trans_rot_t::rot
float rot
Rotation around z-axis.
Definition: types.h:68
fawkes::AbstractDriveMode::stop_at_target_
bool stop_at_target_
flag if stopping on or after target
Definition: abstract_drive_mode.h:100
fawkes::BiwardDriveModule::BiwardDriveModule
BiwardDriveModule(ForwardDriveModule *forward, BackwardDriveModule *backward, Logger *logger, Configuration *config)
Constructor.
Definition: biward_drive_mode.cpp:47
fawkes::AbstractDriveMode::get_proposed_rot
float get_proposed_rot()
Returns the proposed rotatio.
Definition: abstract_drive_mode.h:187
fawkes::colli_trans_rot_t::y
float y
Translation in y-direction.
Definition: types.h:67
fawkes::cart_coord_2d_struct::y
float y
y coordinate
Definition: types.h:66
fawkes::AbstractDriveMode::set_local_target
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
Definition: abstract_drive_mode.h:255
fawkes::AbstractDriveMode::set_current_colli_mode
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
Definition: abstract_drive_mode.h:243
fawkes::AbstractDriveMode
Definition: abstract_drive_mode.h:44
fawkes::colli_trans_rot_t::x
float x
Translation in x-direction.
Definition: types.h:66
fawkes::AbstractDriveMode::local_target_
cart_coord_2d_t local_target_
local target
Definition: abstract_drive_mode.h:96
fawkes::field_pos_t::y
float y
y coordinate in meters
Definition: types.h:126
fawkes::AbstractDriveMode::orient_mode_
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
Definition: abstract_drive_mode.h:99
fawkes
fawkes::field_pos_t::x
float x
x coordinate in meters
Definition: types.h:125
fawkes::AbstractDriveMode::set_current_robo_pos
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
Definition: abstract_drive_mode.h:213
fawkes::BiwardDriveModule::~BiwardDriveModule
~BiwardDriveModule()
Destruct your local values here.
Definition: biward_drive_mode.cpp:68
fawkes::AbstractDriveMode::logger_
Logger * logger_
The fawkes logger.
Definition: abstract_drive_mode.h:106
fawkes::cart_coord_2d_struct::x
float x
x coordinate
Definition: types.h:65
fawkes::AbstractDriveMode::get_proposed_trans_x
float get_proposed_trans_x()
Returns the proposed x translation.
Definition: abstract_drive_mode.h:167
fawkes::AbstractDriveMode::local_trajec_
cart_coord_2d_t local_trajec_
local trajectory
Definition: abstract_drive_mode.h:97
fawkes::AbstractDriveMode::set_current_target
void set_current_target(float x, float y, float ori)
Sets the current target.
Definition: abstract_drive_mode.h:199
fawkes::AbstractDriveMode::robot_vel_
colli_trans_rot_t robot_vel_
current robot velocity
Definition: abstract_drive_mode.h:93
fawkes::AbstractDriveMode::target_
field_pos_t target_
current target
Definition: abstract_drive_mode.h:90
fawkes::field_pos_t::ori
float ori
orientation
Definition: types.h:127
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::AbstractDriveMode::update
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.