|
@ -518,12 +518,10 @@ bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTar |
|
|
void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { |
|
|
void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { |
|
|
// m_zero_shift_x
|
|
|
// m_zero_shift_x
|
|
|
x = motor_pos; |
|
|
x = motor_pos; |
|
|
if (m_param.x_shaft != 0) x = -x; |
|
|
|
|
|
x += m_param.shift_x; |
|
|
x += m_param.shift_x; |
|
|
} |
|
|
} |
|
|
void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { |
|
|
void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { |
|
|
x -= m_param.shift_x; |
|
|
x -= m_param.shift_x; |
|
|
if (m_param.x_shaft != 0) x = -x; |
|
|
|
|
|
motor_pos = x; |
|
|
motor_pos = x; |
|
|
} |
|
|
} |
|
|
#if 0
|
|
|
#if 0
|
|
@ -638,4 +636,5 @@ int32_t StepMotorCtrlModule::factory_reset() { |
|
|
void StepMotorCtrlModule::active_cfg() { |
|
|
void StepMotorCtrlModule::active_cfg() { |
|
|
m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay); |
|
|
m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay); |
|
|
m_stepM1->setScale(m_param.distance_scale); |
|
|
m_stepM1->setScale(m_param.distance_scale); |
|
|
|
|
|
m_stepM1->setMotorShaft(m_param.x_shaft); |
|
|
} |
|
|
} |