23 #include "quadratic_motor_instruct.h"
25 #include <utils/math/common.h>
50 Configuration * config)
53 logger_->log_debug(
"QuadraticMotorInstruct",
"(Constructor): Entering");
54 logger_->log_debug(
"QuadraticMotorInstruct",
"(Constructor): Exiting");
60 logger_->log_debug(
"QuadraticMotorInstruct",
"(Destructor): Entering");
61 logger_->log_debug(
"QuadraticMotorInstruct",
"(Destructor): Exiting");
70 QuadraticMotorInstruct::calculate_translation(
float current,
float desired,
float time_factor)
72 float exec_trans = 0.0;
74 if (desired < current) {
77 exec_trans = current - trans_dec_ - ((
sqr(fabs(current) + 1.0) * trans_dec_) / 8.0);
78 exec_trans = max(exec_trans, desired);
80 }
else if (current < 0.0) {
82 exec_trans = current - trans_acc_ - ((
sqr(fabs(current) + 1.0) * trans_acc_) / 8.0);
83 exec_trans = max(exec_trans, desired);
87 exec_trans = max(-trans_acc_, desired);
90 }
else if (desired > current) {
93 exec_trans = current + trans_acc_ + ((
sqr(fabs(current) + 1.0) * trans_acc_) / 8.0);
94 exec_trans = min(exec_trans, desired);
96 }
else if (current < 0.0) {
98 exec_trans = current + trans_dec_ + ((
sqr(fabs(current) + 1.0) * trans_dec_) / 8.0);
99 exec_trans = min(exec_trans, desired);
103 exec_trans = min(trans_acc_, 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) {
127 exec_rot = current - rot_dec_ - ((
sqr(fabs(current) + 1.0) * rot_dec_) / 8.0);
128 exec_rot = max(exec_rot, desired);
130 }
else if (current < 0.0) {
132 exec_rot = current - rot_acc_ - ((
sqr(fabs(current) + 1.0) * rot_acc_) / 8.0);
133 exec_rot = max(exec_rot, desired);
137 exec_rot = max(-rot_acc_, desired);
140 }
else if (desired > current) {
143 exec_rot = current + rot_acc_ + ((
sqr(fabs(current) + 1.0) * rot_acc_) / 8.0);
144 exec_rot = min(exec_rot, desired);
146 }
else if (current < 0.0) {
148 exec_rot = current + rot_dec_ + ((
sqr(fabs(current) + 1.0) * rot_dec_) / 8.0);
149 exec_rot = min(exec_rot, desired);
153 exec_rot = min(rot_acc_, desired);
161 return exec_rot * time_factor;