24 #include "linear_motor_instruct.h"
26 #include <utils/math/common.h>
51 Configuration * config)
52 : BaseMotorInstruct(motor, frequency, logger, config)
54 logger_->log_debug(
"LinearMotorInstruct",
"(Constructor): Entering");
55 logger_->log_debug(
"LinearMotorInstruct",
"(Constructor): Exiting");
61 logger_->log_debug(
"LinearMotorInstruct",
"(Destructor): Entering");
62 logger_->log_debug(
"LinearMotorInstruct",
"(Destructor): Exiting");
76 LinearMotorInstruct::calculate_translation(
float current,
float desired,
float time_factor)
78 float exec_trans = 0.0;
80 if (desired < current) {
83 exec_trans = current - trans_dec_;
84 exec_trans = max(exec_trans, desired);
86 }
else if (current < 0.0) {
88 exec_trans = current - trans_acc_;
89 exec_trans = max(exec_trans, desired);
93 exec_trans = max(-trans_acc_, desired);
96 }
else if (desired > current) {
99 exec_trans = current + trans_acc_;
100 exec_trans = min(exec_trans, desired);
102 }
else if (current < 0.0) {
104 exec_trans = current + trans_dec_;
105 exec_trans = min(exec_trans, desired);
109 exec_trans = min(trans_acc_, desired);
114 exec_trans = desired;
117 return exec_trans * time_factor;
131 LinearMotorInstruct::calculate_rotation(
float current,
float desired,
float time_factor)
133 float exec_rot = 0.0;
135 if (desired < current) {
138 exec_rot = current - rot_dec_;
139 exec_rot = max(exec_rot, desired);
141 }
else if (current < 0.0) {
143 exec_rot = current - rot_acc_;
144 exec_rot = max(exec_rot, desired);
148 exec_rot = max(-rot_acc_, desired);
151 }
else if (desired > current) {
154 exec_rot = current + rot_acc_;
155 exec_rot = min(exec_rot, desired);
157 }
else if (current < 0.0) {
159 exec_rot = current + rot_dec_;
160 exec_rot = min(exec_rot, desired);
164 exec_rot = min(rot_acc_, desired);
172 return exec_rot * time_factor;