project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
e82611f05a
commit
ec0227ca83
5 changed files with 72 additions and 17 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      components/errorcode/errorcode.hpp
  2. 4
      components/tmc/ic/ztmc4361A.hpp
  3. 50
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  4. 4
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
  5. 15
      components/zcancmder_module/zcan_xy_robot_module.cpp

16
components/errorcode/errorcode.hpp
View File

@ -59,13 +59,15 @@ typedef enum {
* 50000 * 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), // 未找到零点
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), // 未找到零点
kcommon_error_x_leave_away_zero_point_fail = ERROR_CODE(50000, 7), // 离开零点失败
kcommon_error_y_leave_away_zero_point_fail = ERROR_CODE(50000, 8), // 离开零点失败
} error_t; } error_t;
} // namespace err } // namespace err

4
components/tmc/ic/ztmc4361A.hpp
View File

@ -110,8 +110,8 @@ class TMC4361A : public IStepperMotor {
virtual int32_t getVACTUAL(); virtual int32_t getVACTUAL();
virtual int32_t getENC_POS(); virtual int32_t getENC_POS();
virtual void setENC_POS(int32_t value); virtual void setENC_POS(int32_t value);
virtual void setAcceleration(float accelerationpps2); // 设置最大加速度
virtual void setDeceleration(float accelerationpps2); // 设置最大减速度
virtual void setAcceleration(float accelerationpps2); // 设置最大加速度,最大值4000000
virtual void setDeceleration(float accelerationpps2); // 设置最大减速度,最大值4000000
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); } 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 setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); }

50
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -146,9 +146,31 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) {
int32_t XYRobotCtrlModule::exec_move_to_zero_task() { int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
/******************************************************************************* /*******************************************************************************
* 远离X零点 *
*******************************************************************************/
if (m_Xgpio->getState()) {
/**
* @brief 如果设备已经在零点，则反向移动一定距离远离零点
*/
_motor_move_by(m_cfg_runtozero_leave_away_zero_maxXY, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
vTaskDelay(1);
}
if (m_thread.getExitFlag()) {
ZLOGI(TAG, "break move to zero thread exit");
return 0;
}
if (m_Xgpio->getState()) {
ZLOGE(TAG, "leave away zero failed");
return err::kcommon_error_x_leave_away_zero_point_fail;
}
}
/*******************************************************************************
* 移动X轴到零点 * * 移动X轴到零点 *
*******************************************************************************/ *******************************************************************************/
_motor_move_by(m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
_motor_move_by(-m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
bool xreach_zero = false; bool xreach_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
// ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
@ -172,10 +194,34 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
} }
ZLOGI(TAG, "x reach zero"); ZLOGI(TAG, "x reach zero");
/*******************************************************************************
* 远离Y零点 *
*******************************************************************************/
if (m_Ygpio->getState()) {
/**
* @brief 如果设备已经在零点，则反向移动一定距离远离零点
*/
_motor_move_by(0, m_cfg_runtozero_leave_away_zero_maxXY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_dec);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
vTaskDelay(1);
}
if (m_thread.getExitFlag()) {
ZLOGI(TAG, "break move to zero thread exit");
return 0;
}
if (m_Ygpio->getState()) {
ZLOGE(TAG, "leave away zero failed");
return err::kcommon_error_y_leave_away_zero_point_fail;
}
}
/******************************************************************************* /*******************************************************************************
* 移动Y轴到零点 * * 移动Y轴到零点 *
*******************************************************************************/ *******************************************************************************/
_motor_move_by(0, m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
_motor_move_by(0, -m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
bool yreach_zero = false; bool yreach_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
// ZLOGI(TAG, "ygpio %d", m_Ygpio->getState()); // ZLOGI(TAG, "ygpio %d", m_Ygpio->getState());

4
components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
View File

@ -36,8 +36,8 @@ class XYRobotCtrlModule {
int32_t m_cfg_runtozero_maxX = 5120000; int32_t m_cfg_runtozero_maxX = 5120000;
int32_t m_cfg_runtozero_maxY = 5120000; int32_t m_cfg_runtozero_maxY = 5120000;
int32_t m_cfg_runtozero_speed = 30000; 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;
int32_t m_cfg_runtozero_dec = 600000;
int32_t m_cfg_runtozero_leave_away_zero_maxXY = 51200 ;
float m_cfg_distance_scale = 1.0f; float m_cfg_distance_scale = 1.0f;
RobotType_t m_robot_type = corexy; RobotType_t m_robot_type = corexy;

15
components/zcancmder_module/zcan_xy_robot_module.cpp
View File

@ -22,8 +22,10 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
END_PROCESS_PACKET(); END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { // PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { //
errorcode = m_xyRobotCtrlModule->move_to_zero([this, cmdheader](int32_t exec_status, int32_t dx, int32_t dy) { errorcode = m_xyRobotCtrlModule->move_to_zero([this, cmdheader](int32_t exec_status, int32_t dx, int32_t dy) {
osDelay(5); // 用来保证回执消息在前面
ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero exec_status:%d dx:%d dy:%d", exec_status, dx, dy); ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero exec_status:%d dx:%d dy:%d", exec_status, dx, dy);
kcmd_xy_robot_ctrl_move_to_zero_status_report_t report;
kcmd_xy_robot_ctrl_move_to_zero_status_report_t report = {0};
report.id = m_id; report.id = m_id;
report.exec_status = exec_status; report.exec_status = exec_status;
report.dx = dx; report.dx = dx;
@ -35,8 +37,10 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, m_id) { // PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, m_id) { //
errorcode = errorcode =
m_xyRobotCtrlModule->move_to_zero_with_calibrate(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t zero_shift_x, int32_t zero_shift_y) { m_xyRobotCtrlModule->move_to_zero_with_calibrate(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t zero_shift_x, int32_t zero_shift_y) {
osDelay(5); // 用来保证回执消息在前面
ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero_with_calibrate exec_status:%d zero_shift_x:%d zero_shift_y:%d", exec_status, zero_shift_x, zero_shift_y); ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero_with_calibrate exec_status:%d zero_shift_x:%d zero_shift_y:%d", exec_status, zero_shift_x, zero_shift_y);
kcmd_xy_robot_ctrl_move_to_zero_with_calibrate_status_report_t report;
kcmd_xy_robot_ctrl_move_to_zero_with_calibrate_status_report_t report = {0};
report.id = m_id; report.id = m_id;
report.exec_status = exec_status; report.exec_status = exec_status;
report.zero_shift_x = zero_shift_x; report.zero_shift_x = zero_shift_x;
@ -47,8 +51,10 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
END_PROCESS_PACKET(); END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to, m_id) { // PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to, m_id) { //
errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) { errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) {
osDelay(5); // 用来保证回执消息在前面
ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to exec_status:%d tox:%d toy:%d", exec_status, tox, toy); ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to exec_status:%d tox:%d toy:%d", exec_status, tox, toy);
kcmd_xy_robot_ctrl_move_to_status_report_t report;
kcmd_xy_robot_ctrl_move_to_status_report_t report = {0};
report.id = m_id; report.id = m_id;
report.exec_status = exec_status; report.exec_status = exec_status;
report.tox = tox; report.tox = tox;
@ -60,7 +66,8 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { // PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { //
errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) { errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) {
ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d tox:%d toy:%d", exec_status, tox, toy); ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d tox:%d toy:%d", exec_status, tox, toy);
kcmd_xy_robot_ctrl_move_by_status_report_t report;
kcmd_xy_robot_ctrl_move_by_status_report_t report = {0};
report.id = m_id; report.id = m_id;
report.exec_status = exec_status; report.exec_status = exec_status;
report.tox = tox; report.tox = tox;

Write Preview
Loading…
Cancel
Save