Fawkes API  Fawkes Development Version
linear_motor_instruct.cpp
1 
2 /***************************************************************************
3  * linear_motor_instruct.cpp - Motor instructor with linear approximation
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013 Bahram Maleki-Fard
8  * 2014 Tobias Neumann
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "linear_motor_instruct.h"
25 
26 #include <utils/math/common.h>
27 
28 #include <string>
29 
30 namespace fawkes {
31 
32 using namespace std;
33 
34 /** @class LinearMotorInstruct <plugins/colli/drive_realization/linear_motor_instruct.h>
35  * This module is a class for validity checks of drive
36  * commands and sets those things with respect to the physical
37  * borders of the robot.
38  * For this purpose the two functions calculate_rotation and
39  * calculate_translation are implemented linear ;-)
40  */
41 
42 /** Constructor.
43  * @param motor The MotorInterface with all the motor information
44  * @param frequency The frequency of the colli (should become deprecated!)
45  * @param logger The fawkes logger
46  * @param config The fawkes configuration
47  */
48 LinearMotorInstruct::LinearMotorInstruct(MotorInterface *motor,
49  float frequency,
50  Logger * logger,
51  Configuration * config)
52 : BaseMotorInstruct(motor, frequency, logger, config)
53 {
54  logger_->log_debug("LinearMotorInstruct", "(Constructor): Entering");
55  logger_->log_debug("LinearMotorInstruct", "(Constructor): Exiting");
56 }
57 
58 /** Destructor. */
60 {
61  logger_->log_debug("LinearMotorInstruct", "(Destructor): Entering");
62  logger_->log_debug("LinearMotorInstruct", "(Destructor): Exiting");
63 }
64 
65 /** Implementation of Calculate Translation Function.
66  * These are dangerous! Take care while modifying. Only a minus sign too few
67  * or too much may result in non predictable motor behaviour!!!!
68  * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
69  *
70  * @param current The current translation of the robot
71  * @param desired The desired translation of the robot
72  * @param time_factor The time_factor (should become deprecated!)
73  * @return the new translation
74  */
75 float
76 LinearMotorInstruct::calculate_translation(float current, float desired, float time_factor)
77 {
78  float exec_trans = 0.0;
79 
80  if (desired < current) {
81  if (current > 0.0) {
82  // decrease forward speed
83  exec_trans = current - trans_dec_;
84  exec_trans = max(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 = current + trans_dec_;
105  exec_trans = min(exec_trans, desired);
106 
107  } else {
108  // current == 0
109  exec_trans = min(trans_acc_, desired);
110  }
111 
112  } else {
113  // nothing to change!!!
114  exec_trans = desired;
115  }
116 
117  return exec_trans * time_factor;
118 }
119 
120 /** Implementation of Calculate Rotation Function.
121  * These are dangerous! Take care while modifying. Only a minus sign too few
122  * or too much may result in non predictable motor behaviour!!!!
123  * THIS FUNCTION IS THE LAST BORDER TO THE MOTOR, TAKE CARE AND PAY ATTENTION!!!
124  *
125  * @param current The current rotation of the robot
126  * @param desired The desired rotation of the robot
127  * @param time_factor The time_factor (should become deprecated!)
128  * @return the new rotation
129  */
130 float
131 LinearMotorInstruct::calculate_rotation(float current, float desired, float time_factor)
132 {
133  float exec_rot = 0.0;
134 
135  if (desired < current) {
136  if (current > 0.0) {
137  // decrease right rot
138  exec_rot = current - rot_dec_;
139  exec_rot = max(exec_rot, desired);
140 
141  } else if (current < 0.0) {
142  // increase left rot
143  exec_rot = current - rot_acc_;
144  exec_rot = max(exec_rot, desired);
145 
146  } else {
147  // current == 0;
148  exec_rot = max(-rot_acc_, desired);
149  }
150 
151  } else if (desired > current) {
152  if (current > 0.0) {
153  // increase right rot
154  exec_rot = current + rot_acc_;
155  exec_rot = min(exec_rot, desired);
156 
157  } else if (current < 0.0) {
158  // decrease left rot
159  exec_rot = current + rot_dec_;
160  exec_rot = min(exec_rot, desired);
161 
162  } else {
163  // current == 0
164  exec_rot = min(rot_acc_, desired);
165  }
166 
167  } else {
168  // nothing to change!!!
169  exec_rot = desired;
170  }
171 
172  return exec_rot * time_factor;
173 }
174 
175 } // namespace fawkes
fawkes::LinearMotorInstruct::~LinearMotorInstruct
virtual ~LinearMotorInstruct()
Destructor.
Definition: linear_motor_instruct.cpp:64
fawkes
fawkes::LinearMotorInstruct::LinearMotorInstruct
LinearMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
Definition: linear_motor_instruct.cpp:53