23 #include "quadratic_motor_instruct.h" 25 #include <utils/math/common.h> 70 QuadraticMotorInstruct::calculate_translation(
float current,
float desired,
float time_factor)
72 float exec_trans = 0.0;
74 if (desired < current) {
78 exec_trans = max(exec_trans, desired);
80 }
else if (current < 0.0) {
83 exec_trans = max(exec_trans, desired);
90 }
else if (desired > current) {
94 exec_trans = min(exec_trans, desired);
96 }
else if (current < 0.0) {
99 exec_trans = min(exec_trans, desired);
108 exec_trans = desired;
111 return exec_trans * time_factor;
120 QuadraticMotorInstruct::calculate_rotation(
float current,
float desired,
float time_factor)
122 float exec_rot = 0.0;
124 if (desired < current) {
128 exec_rot = max(exec_rot, desired);
130 }
else if (current < 0.0) {
133 exec_rot = max(exec_rot, desired);
140 }
else if (desired > current) {
144 exec_rot = min(exec_rot, desired);
146 }
else if (current < 0.0) {
149 exec_rot = min(exec_rot, desired);
161 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.