22 #include "emergency_motor_instruct.h"
24 #include <config/config.h>
25 #include <interfaces/MotorInterface.h>
26 #include <logging/logger.h>
27 #include <utils/math/common.h>
53 : BaseMotorInstruct(motor, frequency, logger, config)
55 logger_->log_debug(
"EmergencyMotorInstruct",
"(Constructor): Entering");
56 logger_->log_debug(
"EmergencyMotorInstruct",
"(Constructor): Exiting");
62 logger_->log_debug(
"EmergencyMotorInstruct",
"(Destructor): Entering");
63 logger_->log_debug(
"EmergencyMotorInstruct",
"(Destructor): Exiting");
77 EmergencyMotorInstruct::calculate_translation(
float current,
float desired,
float time_factor)
79 float exec_trans = 0.0;
81 if (desired < current) {
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 = desired;
108 exec_trans = min(trans_acc_, desired);
113 exec_trans = desired;
116 return exec_trans * time_factor;
130 EmergencyMotorInstruct::calculate_rotation(
float current,
float desired,
float time_factor)
132 float exec_rot = 0.0;
134 if (desired < current) {
137 exec_rot = current - rot_dec_;
138 exec_rot = max(exec_rot, desired);
140 }
else if (current < 0.0) {
142 exec_rot = current - rot_acc_;
143 exec_rot = max(exec_rot, desired);
147 exec_rot = max(-rot_acc_, desired);
150 }
else if (desired > current) {
153 exec_rot = current + rot_acc_;
154 exec_rot = min(exec_rot, desired);
156 }
else if (current < 0.0) {
158 exec_rot = current + rot_dec_;
159 exec_rot = min(exec_rot, desired);
163 exec_rot = min(rot_acc_, desired);
171 return exec_rot * time_factor;