Browse Source

update

master
zhaohe 1 year ago
parent
commit
40531be4a6
  1. 41
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

41
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@ -173,7 +173,7 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to(int32_t x, int32_t y) {
m_stepM2->setAcceleration(m_cfg.acc);
m_stepM2->setDeceleration(m_cfg.dec);
// ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos);
ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos);
m_stepM1->moveTo(target_m1pos, m_cfg.default_velocity);
m_stepM2->moveTo(target_m2pos, m_cfg.default_velocity);
@ -200,8 +200,13 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() {
m_thread.start([this]() {
befor_motor_move();
exec_move_to_zero_task();
int32_t ecode = exec_move_to_zero_task();
after_motor_move();
if (ecode == 0) {
m_stepM1->setXACTUAL(0);
m_stepM2->setXACTUAL(0);
}
return;
});
return 0;
@ -316,12 +321,34 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
}
void XYRobotCtrlModule::_motor_move_to_end(int32_t xdirection, int32_t ydirection, int32_t maxv) {
ZLOGI(TAG, "_motor_move_to_end xdirection:%d ydirection:%d maxv:%d acc:%d dec:%d ", xdirection, ydirection, maxv);
if (xdirection != 0) {
m_stepM1->rotate(xdirection * maxv);
} else if (ydirection != 0) {
m_stepM2->rotate(ydirection * maxv);
ZLOGI(TAG, "_motor_move_to_end xdirection:%d ydirection:%d maxv:%d ", xdirection, ydirection, maxv);
int32_t dx = 0, dy = 0;
if (xdirection > 0) {
dx = 100000;
} else if (xdirection < 0) {
dx = -100000;
} else {
dx = 0;
}
if (ydirection > 0) {
dy = 100000;
} else if (ydirection < 0) {
dy = -100000;
} else {
dy = 0;
}
int32_t t_x, t_y;
getnowpos(t_x, t_y);
t_x = t_x + dx;
t_y = t_y + dy;
int32_t target_m1pos, target_m2pos;
forward_kinematics(t_x, t_y, target_m1pos, target_m2pos);
m_stepM1->moveTo(target_m1pos, maxv);
m_stepM2->moveTo(target_m2pos, maxv);
}
bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); }
void XYRobotCtrlModule::_motor_stop() {

Loading…
Cancel
Save