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