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;