23 #include "quadratic_motor_instruct.h" 25 #include <utils/math/common.h> 76 QuadraticMotorInstruct::calculate_translation(
float current,
float desired,
float time_factor )
78 float exec_trans = 0.0;
80 if (desired < current) {
85 exec_trans = max( exec_trans, desired );
87 }
else if (current < 0.0) {
90 exec_trans = max( exec_trans, desired );
97 }
else if (desired > current) {
102 exec_trans = min( exec_trans, desired );
104 }
else if (current < 0.0) {
107 exec_trans = min( exec_trans, desired );
116 exec_trans = desired;
119 return exec_trans*time_factor;
128 float QuadraticMotorInstruct::calculate_rotation(
float current,
float desired,
float time_factor )
130 float exec_rot = 0.0;
132 if (desired < current) {
137 exec_rot = max( exec_rot, desired );
139 }
else if (current < 0.0) {
142 exec_rot = max( exec_rot, desired );
146 exec_rot = max( -
rot_acc_, desired );
149 }
else if (desired > current) {
153 exec_rot = min( exec_rot, desired );
155 }
else if (current < 0.0) {
158 exec_rot = min( exec_rot, desired );
162 exec_rot = min(
rot_acc_, desired );
170 return exec_rot*time_factor;
Fawkes library namespace.
QuadraticMotorInstruct(MotorInterface *motor, float frequency, Logger *logger, Configuration *config)
Constructor.
float trans_dec_
Translation deceleration.
double sqr(double x)
Fast square multiplication.
virtual ~QuadraticMotorInstruct()
Destructor.
float rot_acc_
Rotation acceleration.
float rot_dec_
Rotation deceleration.
Logger * logger_
The fawkes logger.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float trans_acc_
Translation acceleration.
The Basic of a Motorinstructor.
MotorInterface Fawkes BlackBoard Interface.
Interface for configuration handling.