diff --git a/components/errorcode/errorcode.hpp b/components/errorcode/errorcode.hpp new file mode 100644 index 0000000..cb2a5ce --- /dev/null +++ b/components/errorcode/errorcode.hpp @@ -0,0 +1,72 @@ +#pragma once + +namespace iflytop { +namespace err { +using namespace std; + +#define ERROR_CODE(errortype, suberrorcode) (errortype + suberrorcode) + +typedef enum { + ksucc = ERROR_CODE(0, 0), + kfail = ERROR_CODE(0, 1), + + /** + * @brief 通信错误 + */ + kce_overtime = ERROR_CODE(1000, 0), + kce_noack = ERROR_CODE(1000, 1), + kce_errorack = ERROR_CODE(1000, 2), + kce_device_offline = ERROR_CODE(1000, 3), + kce_parse_json_err = ERROR_CODE(1000, 4), + + /** + * @brief 数据库错误 + */ + kdbe_user_not_exist = ERROR_CODE(2000, 0), + kdbe_catch_exception = ERROR_CODE(2000, 1), + + /** + * @brief 硬件错误 + */ + kharde_unfound = ERROR_CODE(3000, 0), + /** + * @brief 程序运行错误 + */ + kre_catch_exception = ERROR_CODE(4000, 0), + /** + * @brief 应用交互基本错误 + */ + kinteraction_error_passwd_error = ERROR_CODE(5000, 0), // 密码错误 + kinteraction_error_user_not_exist = ERROR_CODE(5000, 1), // 用户不存在 + /** + * @brief 系统错误 + */ + ksys_error = ERROR_CODE(6000, 0), + ksys_create_file_error = ERROR_CODE(6000, 1), + ksys_create_dir_error = ERROR_CODE(6000, 2), + ksys_open_file_error = ERROR_CODE(6000, 3), + ksys_open_dir_error = ERROR_CODE(6000, 4), + ksys_read_file_error = ERROR_CODE(6000, 5), + ksys_write_file_error = ERROR_CODE(6000, 6), + ksys_close_file_error = ERROR_CODE(6000, 7), + ksys_close_dir_error = ERROR_CODE(6000, 8), + ksys_delete_file_error = ERROR_CODE(6000, 9), + ksys_delete_dir_error = ERROR_CODE(6000, 10), + ksys_copy_file_error = ERROR_CODE(6000, 11), + + /** + * @brief 通用，但未归类错误 + * 50000 + */ + + kcommon_error_device_not_zero = ERROR_CODE(50000, 0), // 设备未归零 + kcommon_error_over_temperature = ERROR_CODE(50000, 1), // 过温 + kcommon_error_over_voltage = ERROR_CODE(50000, 2), // 过压 + kcommon_error_param_out_of_range = ERROR_CODE(50000, 3), // 参数超出范围 + kcommon_error_not_found_zero_point = ERROR_CODE(50000, 4), // 未找到零点 + kcommon_error_not_found_x_zero_point = ERROR_CODE(50000, 5), // 未找到零点 + kcommon_error_not_found_y_zero_point = ERROR_CODE(50000, 6), // 未找到零点 + +} error_t; +} // namespace err +} // namespace iflytop \ No newline at end of file diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp index 253d2fb..570c54d 100644 --- a/components/tmc/basic/tmc_ic_interface.hpp +++ b/components/tmc/basic/tmc_ic_interface.hpp @@ -58,8 +58,9 @@ class IStepperMotor { virtual bool isStoped() = 0; virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0; + virtual void setDistanceK(float distanceK) = 0; - virtual void enable(bool enable) = 0; + virtual void enable(bool enable) = 0; }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index 09435b9..832496a 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -141,6 +141,7 @@ void TMC4361A::setAcceleration(float accelerationpps2) { * AMAX [pps2] = AMAX / 237 ?? fCLK^2 */ + accelerationpps2 *= m_distanceK; int32_t acc = (int32_t)accelerationpps2; writeInt(TMC4361A_AMAX, acc << 2); } @@ -155,7 +156,7 @@ void TMC4361A::setDeceleration(float accelerationpps2) { * a[?v per clk_cycle]= AMAX / 2^37 * AMAX [pps2] = AMAX / 237 ?? fCLK^2 */ - + accelerationpps2 *= m_distanceK; int32_t acc = (int32_t)accelerationpps2; writeInt(TMC4361A_DMAX, acc << 2); } @@ -197,8 +198,11 @@ void TMC4361A::initialize(cfg_t *cfg) { zchip_clock_early_delayus(300 * 1000); zchip_clock_early_delayus(300 * 1000); - driverIC_setIHOLD_IRUN(1, 3, 0); + driverIC_setIHOLD_IRUN(1, 3, 0); } + +void TMC4361A::setDistanceK(float distanceK) { m_distanceK = distanceK; } + uint8_t TMC4361A::reset() { // Pulse the low-active hardware reset pin stop(); @@ -243,13 +247,13 @@ uint8_t TMC4361A::reset() { } uint8_t TMC4361A::restore() { return 1; } -int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL); } -void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value); } -int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL); } -int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET); } -int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS); } -void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value); } -int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD); } +int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL) / m_distanceK; } +void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value * m_distanceK); } +int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL) / m_distanceK; } +int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET) / m_distanceK; } +int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS) / m_distanceK; } +void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value * m_distanceK); } +int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD) / m_distanceK; } void TMC4361A::enableIC(bool enable) { SET_PIN(m_ennPin, !enable); SET_PIN(m_driverIC_ennPin, !enable); @@ -262,12 +266,16 @@ int32_t tmc4361A_discardVelocityDecimals(int32_t value) { return value << 8; } void TMC4361A::rotate(int32_t velocity) { + velocity *= m_distanceK; m_motor_mode = kvelmode; PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0); writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity)); } void TMC4361A::stop() { rotate(0); } void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) { + position *= m_distanceK; + velocityMax *= m_distanceK; + m_motor_mode = kposmode; PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1); writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax)); @@ -300,6 +308,7 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) { } bool TMC4361A::isReachTarget() { uint32_t value = readInt(TMC4361A_STATUS); + // printf("TMC4361A_STATUS:%08x\n", value); if (m_motor_mode == kposmode) { return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0; } else { diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index 6c4a2e0..de84bc6 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -74,6 +74,8 @@ class TMC4361A : public IStepperMotor { motor_mode_t m_motor_mode = kvelmode; + float m_distanceK = 1.0; + public: TMC4361A(/* args */); @@ -114,6 +116,8 @@ class TMC4361A : public IStepperMotor { virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); } virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); } + virtual void setDistanceK(float distanceK); + /******************************************************************************* * 驱动器初始化方法 * *******************************************************************************/ diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 5cdd0ce..5c9c9d7 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -1,4 +1,6 @@ #include "xy_robot_ctrl_module.hpp" + +#include "sdk/components/errorcode/errorcode.hpp" using namespace iflytop; using namespace std; #define TAG "XYRobotCtrlModule" @@ -7,91 +9,295 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // ZGPIO* Xgpio, // ZGPIO* Ygpio, // float distanceK) { - m_stepM1 = stepM1; - m_stepM2 = stepM2; - m_Xgpio = Xgpio; - m_Ygpio = Ygpio; - m_distanceK = distanceK; + m_stepM1 = stepM1; + m_stepM2 = stepM2; + m_Xgpio = Xgpio; + m_Ygpio = Ygpio; + ZASSERT(m_stepM1); + ZASSERT(m_stepM2); + ZASSERT(m_Xgpio); + ZASSERT(m_Ygpio); + m_stepM1->setDistanceK(distanceK); + m_stepM2->setDistanceK(distanceK); + m_lock.init(); + m_statu_lock.init(); m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); } +void XYRobotCtrlModule::dumpcfg(const char* tag) { + ZLOGI(TAG, "======dumpcfg:%s=========", tag); + ZLOGI(TAG, "= m_cfg_acc :%d", m_cfg_acc); + ZLOGI(TAG, "= m_cfg_dec :%d", m_cfg_dec); + ZLOGI(TAG, "= m_cfg_velocity :%d", m_cfg_velocity); + ZLOGI(TAG, "= m_cfg_runtozero_maxX :%d", m_cfg_runtozero_maxX); + ZLOGI(TAG, "= m_cfg_runtozero_maxY :%d", m_cfg_runtozero_maxY); + ZLOGI(TAG, "= m_cfg_runtozero_speed :%d", m_cfg_runtozero_speed); + ZLOGI(TAG, "= m_cfg_runtozero_dec :%d", m_cfg_runtozero_dec); + ZLOGI(TAG, "="); +} +int32_t XYRobotCtrlModule::set_zero_robottype(RobotType_t robot_type) { + m_robot_type = robot_type; + return 0; +} +int32_t XYRobotCtrlModule::set_x_shaft(bool x_shaft) { + m_x_shaft = x_shaft; + return 0; +} +int32_t XYRobotCtrlModule::set_y_shaft(bool y_shaft) { + m_y_shaft = y_shaft; + return 0; +} + int32_t XYRobotCtrlModule::enable(bool venable) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "enable:%d", venable); m_stepM1->enable(venable); m_stepM2->enable(venable); return 0; } -int32_t XYRobotCtrlModule::getMotor1Pos() { return m_stepM1->getXACTUAL() * m_distanceK; } -int32_t XYRobotCtrlModule::getMotor2Pos() { return m_stepM2->getXACTUAL() * m_distanceK; } - -void XYRobotCtrlModule::getNowPos(int32_t& x, int32_t& y) { - // y=M1+M2; - // X=m1-m2; - x = getMotor1Pos() - getMotor2Pos(); - y = getMotor1Pos() + getMotor2Pos(); -} - int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y) { + zlock_guard lock(m_lock); ZLOGI(TAG, "move_to x:%d y:%d", x, y); int32_t m1pos, m2pos; - inverse_kinematics(x, y, m1pos, m2pos); - m_thread.stop(); - m_thread.start([this,m1pos,m2pos]() { - ZLOGI("XYRobotCtrlModule", "m1pos:%d m2pos:%d", m1pos, m2pos); + /** + * @TODO:添加限位检查 + */ + updateStatus(1); + m_thread.start([this, x, y]() { + _motor_move_to(x, y, m_cfg_velocity, m_cfg_acc, m_cfg_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + _motor_stop(); + updateStatus(0); }); return 0; } + int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy) { + zlock_guard lock(m_lock); ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy); - return 0; + int32_t now_x, now_y; + getnowpos(now_x, now_y); + return move_to(now_x + dx, now_y + dy); } + int32_t XYRobotCtrlModule::move_to_zero() { + zlock_guard lock(m_lock); ZLOGI(TAG, "move_to_zero"); + + updateStatus(1); + m_thread.stop(); + m_thread.start([this]() { + int32_t erro = exec_move_to_zero_task(); + if (erro != 0) { + ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); + updateStatus(0); + return; + } + m_stepM1->setXACTUAL(0); + m_stepM2->setXACTUAL(0); + updateStatus(0); + return; + }); + return 0; } int32_t XYRobotCtrlModule::move_to_with_calibrate(int32_t x, int32_t y) { + zlock_guard lock(m_lock); ZLOGI(TAG, "move_to_with_calibrate x:%d y:%d", x, y); return 0; } int32_t XYRobotCtrlModule::stop() { + zlock_guard lock(m_lock); ZLOGI(TAG, "stop"); + m_thread.stop(); return 0; } - uint8_t XYRobotCtrlModule::read_status(uint8_t* module_statu, int32_t* x, int32_t* y) { return 0; } - int32_t XYRobotCtrlModule::set_acc(int32_t acc) { + zlock_guard lock(m_lock); ZLOGI(TAG, "set_acc acc:%d", acc); + m_cfg_acc = acc; return 0; } int32_t XYRobotCtrlModule::set_dec(int32_t dec) { + zlock_guard lock(m_lock); ZLOGI(TAG, "set_dec dec:%d", dec); + m_cfg_dec = dec; return 0; } int32_t XYRobotCtrlModule::set_speed(int32_t speed) { + zlock_guard lock(m_lock); ZLOGI(TAG, "set_speed speed:%d", speed); + m_cfg_velocity = speed; + return 0; +} +int32_t XYRobotCtrlModule::set_runtozero_maxX(int32_t maxX) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "set_runtozero_maxX:%d", maxX); + m_cfg_runtozero_maxX = maxX; + return 0; +} +int32_t XYRobotCtrlModule::set_runtozero_maxY(int32_t maxY) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "set_runtozero_maxY:%d", maxY); + m_cfg_runtozero_maxY = maxY; + return 0; +} +int32_t XYRobotCtrlModule::set_runtozero_speed(int32_t speed) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "set_runtozero_speed:%d", speed); + m_cfg_runtozero_speed = speed; + return 0; +} +int32_t XYRobotCtrlModule::set_runtozero_dec(int32_t dec) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "set_runtozero_dec:%d", dec); + m_cfg_runtozero_dec = dec; + return 0; +} + +int32_t XYRobotCtrlModule::set_runtozero_leave_away_zero_maxXY(int32_t maxXY) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "set_runtozero_leave_away_zero_maxXY:%d", maxXY); + m_cfg_runtozero_leave_away_zero_maxXY = maxXY; + return 0; +} +int32_t XYRobotCtrlModule::set_distance_scale(float distance_scale) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "set_distance_scale:%f", distance_scale); + m_stepM1->setDistanceK(distance_scale); + m_stepM2->setDistanceK(distance_scale); return 0; } void XYRobotCtrlModule::loop() {} -void XYRobotCtrlModule::forward_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { -#if 1 - x = m1pos - m2pos; - y = m1pos + m2pos; -#else - x = m1pos + m2pos; - y = m1pos - m2pos; -#endif -} -void XYRobotCtrlModule::inverse_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { -#if 1 - m1pos = (x + y) / 2; - m2pos = (y - x) / 2; -#else - m1pos = (x - y) / 2; - m2pos = (x + y) / 2; -#endif -} \ No newline at end of file +int32_t XYRobotCtrlModule::exec_move_to_zero_task() { + /******************************************************************************* + * 移动X轴到零点 * + *******************************************************************************/ + _motor_move_by(m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + bool xreach_zero = false; + while (true) { + // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); + if (m_Xgpio->getState()) { + xreach_zero = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + + if (!xreach_zero) { + // 触发异常回调 + ZLOGE(TAG, "x reach zero failed"); + return err::kcommon_error_not_found_x_zero_point; + } + ZLOGI(TAG, "x reach zero"); + /******************************************************************************* + * 移动Y轴到零点 * + *******************************************************************************/ + _motor_move_by(0, m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + bool yreach_zero = false; + while (!_motor_is_reach_target()) { + // ZLOGI(TAG, "ygpio %d", m_Ygpio->getState()); + if (m_Ygpio->getState()) { + yreach_zero = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + if (!yreach_zero) { + // 触发异常回调 + ZLOGE(TAG, "y reach zero failed"); + return err::kcommon_error_not_found_y_zero_point; + } + ZLOGI(TAG, "y reach zero"); + ZLOGI(TAG, "move_to_zero success"); + return 0; +} + +void XYRobotCtrlModule::updateStatus(uint8_t status) { + zlock_guard lock(m_statu_lock); + m_status = status; +} +void XYRobotCtrlModule::getnowpos(int32_t& x, int32_t& y) { + int32_t m1pos, m2pos; + m1pos = m_stepM1->getXACTUAL(); + m2pos = m_stepM1->getXACTUAL(); + inverse_kinematics(m1pos, m2pos, x, y); +} +void XYRobotCtrlModule::_motor_move_to(int32_t x, int32_t y, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "_motor_move_to x:%d y:%d maxv:%d,acc:%d,dec:%d", x, y, maxv, acc, dec); + int32_t target_m1pos, target_m2pos; + forward_kinematics(x, y, target_m1pos, target_m2pos); + m_stepM1->setAcceleration(acc); + m_stepM2->setAcceleration(acc); + + m_stepM1->setDeceleration(dec); + m_stepM2->setDeceleration(dec); + + m_stepM1->moveTo(target_m1pos, maxv); + m_stepM2->moveTo(target_m2pos, maxv); +} + +void XYRobotCtrlModule::_motor_move_by(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec) { + // ZLOGI(TAG, "_motor_move_by dx:%d dy:%d", dx, dy); + ZLOGI(TAG, "_motor_move_by dx:%d dy:%d maxv:%d acc:%d dec:%d ", dx, dy, maxv, acc, dec); + 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->setAcceleration(acc); + m_stepM2->setAcceleration(acc); + + m_stepM1->setDeceleration(dec); + m_stepM2->setDeceleration(dec); + ZLOGI(TAG, "_motor_move_by target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos); + m_stepM1->moveTo(target_m1pos, maxv); + m_stepM2->moveTo(target_m2pos, maxv); +} +void XYRobotCtrlModule::_motor_stop(int32_t dec) { + if (dec > 0) { + m_stepM1->setDeceleration(dec); + m_stepM2->setDeceleration(dec); + } + m_stepM1->stop(); + m_stepM2->stop(); +} +void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { + if (m_robot_type == corexy) { + x = (m1pos - m2pos); + y = (m1pos + m2pos); + } else { + x = m1pos + m2pos; + y = m1pos - m2pos; + } + if (m_x_shaft) x = -x; + if (m_y_shaft) y = -y; +} +void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { + if (m_x_shaft) x = -x; + if (m_y_shaft) y = -y; + + if (m_robot_type == corexy) { + m1pos = ((x + y) / 2); + m2pos = ((y - x) / 2); + } else { + m1pos = (x - y) / 2; + m2pos = (x + y) / 2; + } +} + +bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); } diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 7a5c946..7e6dbd6 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -6,6 +6,10 @@ namespace iflytop { class XYRobotCtrlModule { + public: + typedef enum { hbot, corexy } RobotType_t; + + private: IStepperMotor* m_stepM1; IStepperMotor* m_stepM2; @@ -14,11 +18,31 @@ class XYRobotCtrlModule { ZThread m_thread; - float m_distanceK = 1.0f; - int m_x = 0; int m_y = 0; + uint8_t m_status = 0; + + int32_t m_zero_shift_x = 0; + int32_t m_zero_shift_y = 0; + + zmutex m_lock; + zmutex m_statu_lock; + + int32_t m_cfg_acc = 300000; + int32_t m_cfg_dec = 300000; + int32_t m_cfg_velocity = 300000; + + int32_t m_cfg_runtozero_maxX = 5120000; + int32_t m_cfg_runtozero_maxY = 5120000; + int32_t m_cfg_runtozero_speed = 30000; + int32_t m_cfg_runtozero_dec = 300000; + int32_t m_cfg_runtozero_leave_away_zero_maxXY = 51200 * 5; + + RobotType_t m_robot_type = corexy; + bool m_x_shaft = false; + bool m_y_shaft = false; + public: void initialize(IStepperMotor* stepM1, // IStepperMotor* stepM2, // @@ -26,6 +50,22 @@ class XYRobotCtrlModule { ZGPIO* Ygpio, // float distanceK); + int32_t set_acc(int32_t acc); + int32_t set_dec(int32_t dec); + int32_t set_speed(int32_t speed); + int32_t set_zero_robottype(RobotType_t robot_type); + int32_t set_x_shaft(bool x_shaft); + int32_t set_y_shaft(bool y_shaft); + + int32_t set_runtozero_maxX(int32_t maxX); + int32_t set_runtozero_maxY(int32_t maxY); + int32_t set_runtozero_speed(int32_t speed); + int32_t set_runtozero_dec(int32_t dec); + int32_t set_runtozero_leave_away_zero_maxXY(int32_t maxXY); + int32_t set_distance_scale(float distance_scale); + + void dumpcfg(const char* tag); + int32_t enable(bool venable); int32_t move_to(int32_t x, int32_t y); @@ -36,20 +76,22 @@ class XYRobotCtrlModule { uint8_t read_status(uint8_t* module_statu, int32_t* x, int32_t* y); - int32_t set_acc(int32_t acc); - int32_t set_dec(int32_t dec); - int32_t set_speed(int32_t speed); - void loop(); private: - int32_t getMotor1Pos(); - int32_t getMotor2Pos(); + void updateStatus(uint8_t status); - void getNowPos(int32_t& x, int32_t& y); void computeTargetMotorPos(); + void getnowpos(int32_t& x, int32_t& y); + + int32_t exec_move_to_zero_task(); + + void inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y); + void forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos); - void forward_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y); - void inverse_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos); + void _motor_move_to(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec); + void _motor_move_by(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec); + void _motor_stop(int32_t dec = -1); + bool _motor_is_reach_target(); }; } // namespace iflytop \ No newline at end of file diff --git a/components/zcancmder/cmd/basic_bean.hpp b/components/zcancmder/cmd/basic_bean.hpp index 921ab54..c434122 100644 --- a/components/zcancmder/cmd/basic_bean.hpp +++ b/components/zcancmder/cmd/basic_bean.hpp @@ -66,16 +66,27 @@ typedef enum { /******************************************************************************* * 1006 * *******************************************************************************/ - kcmd_xy_robot_ctrl_enable = CMDID(1006, 0), - kcmd_xy_robot_ctrl_stop = CMDID(1006, 1), - kcmd_xy_robot_ctrl_move_to_zero = CMDID(1006, 2), - kcmd_xy_robot_ctrl_move_to_with_calibrate = CMDID(1006, 3), - kcmd_xy_robot_ctrl_move_to = CMDID(1006, 4), - kcmd_xy_robot_ctrl_move_by = CMDID(1006, 5), - kcmd_xy_robot_ctrl_read_status = CMDID(1006, 50), - kcmd_xy_robot_ctrl_set_acc = CMDID(1006, 100), - kcmd_xy_robot_ctrl_set_dec = CMDID(1006, 101), - kcmd_xy_robot_ctrl_set_speed = CMDID(1006, 102), + kcmd_xy_robot_ctrl_enable = CMDID(1006, 0), + kcmd_xy_robot_ctrl_stop = CMDID(1006, 1), + kcmd_xy_robot_ctrl_move_to_zero = CMDID(1006, 2), + kcmd_xy_robot_ctrl_move_to_with_calibrate = CMDID(1006, 3), + kcmd_xy_robot_ctrl_move_to = CMDID(1006, 4), + kcmd_xy_robot_ctrl_move_by = CMDID(1006, 5), + kcmd_xy_robot_ctrl_read_status = CMDID(1006, 50), + kcmd_xy_robot_ctrl_set_acc = CMDID(1006, 100), + kcmd_xy_robot_ctrl_set_dec = CMDID(1006, 101), + kcmd_xy_robot_ctrl_set_speed = CMDID(1006, 102), + kcmd_xy_robot_ctrl_set_x_shaft = CMDID(1006, 103), + kcmd_xy_robot_ctrl_set_y_shaft = CMDID(1006, 104), + kcmd_xy_robot_ctrl_set_zero_robottype = CMDID(1006, 105), + kcmd_xy_robot_ctrl_set_runtozero_maxX = CMDID(1006, 106), + kcmd_xy_robot_ctrl_set_runtozero_maxY = CMDID(1006, 107), + kcmd_xy_robot_ctrl_set_runtozero_speed = CMDID(1006, 108), + kcmd_xy_robot_ctrl_set_runtozero_dec = CMDID(1006, 109), + kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY = CMDID(1006, 110), + kcmd_xy_robot_ctrl_set_distance_scale = CMDID(1006, 111), + // + } CmdID_t; } // namespace zcr diff --git a/components/zcancmder/cmd/c1006_module_cmd.hpp b/components/zcancmder/cmd/c1006_module_cmd.hpp index 8ee55c7..69089fe 100644 --- a/components/zcancmder/cmd/c1006_module_cmd.hpp +++ b/components/zcancmder/cmd/c1006_module_cmd.hpp @@ -22,6 +22,16 @@ ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, cmd, uint8_t id; uint8_t _pad;); ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, cmd, uint8_t id; uint8_t _pad; int32_t acc;); ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;); ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_x_shaft, cmd, uint8_t id; uint8_t _pad; uint8_t x_shaft;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_y_shaft, cmd, uint8_t id; uint8_t _pad; uint8_t y_shaft;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_zero_robottype, cmd, uint8_t id; uint8_t _pad; uint8_t zero_robottype;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxX, cmd, uint8_t id; uint8_t _pad; int32_t maxX;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxY, cmd, uint8_t id; uint8_t _pad; int32_t maxY;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY, cmd, uint8_t id; uint8_t _pad; int32_t maxXY;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, cmd, uint8_t id; uint8_t _pad; float distance_scale;); + /******************************************************************************* * ACK * *******************************************************************************/ @@ -35,6 +45,16 @@ ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, ack, uint8_t id; uint8_t module_s ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, ack, uint8_t id; uint8_t _pad;); ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, ack, uint8_t id; uint8_t _pad;); ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_x_shaft, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_y_shaft, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_zero_robottype, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxX, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxY, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY, ack, uint8_t id; uint8_t _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, ack, uint8_t id; uint8_t _pad;); + /******************************************************************************* * status_report * *******************************************************************************/ diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index a81d066..63091d5 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -50,4 +50,40 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { errorcode = m_xyRobotCtrlModule->set_speed(cmd->speed); } END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_x_shaft, m_id) { // + errorcode = m_xyRobotCtrlModule->set_x_shaft(cmd->x_shaft); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_y_shaft, m_id) { // + errorcode = m_xyRobotCtrlModule->set_y_shaft(cmd->y_shaft); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_zero_robottype, m_id) { // + errorcode = m_xyRobotCtrlModule->set_zero_robottype((XYRobotCtrlModule::RobotType_t)cmd->zero_robottype); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_maxX, m_id) { // + errorcode = m_xyRobotCtrlModule->set_runtozero_maxX(cmd->maxX); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_maxY, m_id) { // + errorcode = m_xyRobotCtrlModule->set_runtozero_maxY(cmd->maxY); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_speed, m_id) { // + errorcode = m_xyRobotCtrlModule->set_runtozero_speed(cmd->speed); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_dec, m_id) { // + errorcode = m_xyRobotCtrlModule->set_runtozero_dec(cmd->dec); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY, m_id) { // + errorcode = m_xyRobotCtrlModule->set_runtozero_leave_away_zero_maxXY(cmd->maxXY); + } + END_PROCESS_PACKET(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_distance_scale, m_id) { // + errorcode = m_xyRobotCtrlModule->set_distance_scale(cmd->distance_scale); + } + END_PROCESS_PACKET(); } \ No newline at end of file diff --git a/os/zthread.cpp b/os/zthread.cpp index 1cfb2b1..6d5f041 100644 --- a/os/zthread.cpp +++ b/os/zthread.cpp @@ -44,6 +44,8 @@ void ZThread::init(const char *threadname, int stack_size, osPriority priority) ZASSERT(m_defaultTaskHandle != NULL); } void ZThread::start(zosthread_cb_t cb) { + stop(); + m_taskfunction = cb; ZASSERT(m_taskfunction); diff --git a/os/zthread.hpp b/os/zthread.hpp index 4cb711f..00acfb9 100644 --- a/os/zthread.hpp +++ b/os/zthread.hpp @@ -26,6 +26,8 @@ class ZThread { void start(zosthread_cb_t cb); void stop(); + bool getExitFlag() { return !m_threadisworkingFlagCallSide; } + void sleep(uint32_t ms); void wake(); };