From 031f4de05c5ee14bea7752d26c38789517ac3d04 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 14 Oct 2023 23:02:46 +0800 Subject: [PATCH] update --- components/eq_20_asb_motor/eq20_servomotor.cpp | 16 +++- components/eq_20_asb_motor/eq20_servomotor.hpp | 94 +++++++++++----------- .../script_cmder_eq20_servomotor.cpp | 28 ++++++- 3 files changed, 85 insertions(+), 53 deletions(-) diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index 8733e6e..e7c0504 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -43,7 +43,8 @@ write_pn 1 617 100 #Pn617 write_pn_bit 1 1501 0 0 write_pn_bit 1 1501 0 1 #endif - DO(write_pn(610, 0x0001)); + DO(stop()) + DO(write_pn(610, 0x0011)); DO(write_pn(615, rpm)); DO(write_pn(614, topos)); DO(write_pn(616, acctime)); @@ -62,7 +63,8 @@ write_pn 1 617 100 #Pn617 write_pn_bit 1 1501 0 0 write_pn_bit 1 1501 0 1 #endif - DO(write_pn(610, 0x0011)); + DO(stop()) + DO(write_pn(610, 0x0001)); DO(write_pn(614, pos)); DO(write_pn(615, rpm)); DO(write_pn(616, acctime)); @@ -71,7 +73,11 @@ write_pn_bit 1 1501 0 1 DO(write_pn_bit(1501, 0, 1)); return 0; } -int32_t Eq20ServoMotor::rotate(int32_t rpm, int32_t acctime) { return move_by(100000 * 10000, rpm, acctime); } +int32_t Eq20ServoMotor::rotate(int32_t rpm, int32_t acctime) { + DO(stop()); + if (rpm == 0) return 0; + return move_by(100000 * 10000, rpm, acctime); +} int32_t Eq20ServoMotor::move_to_zero_forward(int32_t lookzeropoint_rpm, int32_t findzero_edge_rpm, int32_t lookzeropoint_acc_time) { #if 0 @@ -85,6 +91,7 @@ write_pn 1 606 0 # write_pn_bit 1 1501 1 0 write_pn_bit 1 1501 1 1 #endif + DO(stop()) DO(write_pn(600, 11)); DO(write_pn(601, lookzeropoint_rpm)); DO(write_pn(602, findzero_edge_rpm)); @@ -97,6 +104,7 @@ write_pn_bit 1 1501 1 1 return 0; } int32_t Eq20ServoMotor::move_to_zero_backward(int32_t lookzeropoint_rpm, int32_t findzero_edge_rpm, int32_t lookzeropoint_acc_time) { + DO(stop()) DO(write_pn(600, 8)); DO(write_pn(601, lookzeropoint_rpm)); DO(write_pn(602, findzero_edge_rpm)); @@ -126,7 +134,7 @@ int32_t Eq20ServoMotor::stop() { } int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) { - DO(read_pn(1065, state.status)); + DO(read_pn(1065, (int32_t &)state.data.status)); return 0; } diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index 38ac4c8..425af18 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -10,54 +10,56 @@ using namespace std; class Eq20ServoMotor { public: typedef struct { - int32_t status; + union { + // bit0:同服准备好 + // bitl:伺服报警 + // bit2:电机旋转 + // bit3:定位完成 + // bit4:速度一致 + // bit5:电机零速 + // bit6: 制动器(抱闸) + // bit7:定位接近 + // bit8:转矩受限 + // bit9:速度受限 + // bit10:转矩检测 + // bit11:伺服警告 + // bit12:伺服故障代码 1 + // bit13:伺服故障代码 2 + // bit14:伺服故障代码 3 + // bit15:回零完成 + // bit16:编码器 C 脉冲有效 + // bit17:电机励磁 + // bit18:超程状态 + // bit19 编码器电池警告 + // bit20:编码器电池报数 + uint32_t status; + struct { + uint32_t servo_is_ready : 1; + uint32_t servo_warning : 1; + uint32_t motor_is_rotating : 1; + uint32_t is_reach_target : 1; + uint32_t is_speed_consistent : 1; + uint32_t motor_is_zero_speed : 1; + uint32_t brake_is_on : 1; + uint32_t is_pos_close : 1; + uint32_t torque_is_limited : 1; + uint32_t speed_is_limited : 1; + uint32_t torque_is_detected : 1; + uint32_t servo_warning2 : 1; + uint32_t servo_error_code1 : 1; + uint32_t servo_error_code2 : 1; + uint32_t servo_error_code3 : 1; + uint32_t is_zero_complete : 1; + uint32_t encoder_c_pulse_is_valid : 1; + uint32_t motor_is_excited : 1; + uint32_t is_over_travel : 1; + uint32_t encoder_battery_warning : 1; + uint32_t encoder_battery_report : 1; + uint32_t padding : 11; + } sbit; + } data; } servo_internal_status_t; - // bit0:同服准备好 - // bitl:伺服报警 - // bit2:电机旋转 - // bit3:定位完成 - // bit4:速度一致 - // bit5:电机零速 - // bit6: 制动器(抱闸) - // bit7:定位接近 - // bit8:转矩受限 - // bit9:速度受限 - // bit10:转矩检测 - // bit11:伺服警告 - // bit12:伺服故障代码 1 - // bit13:伺服故障代码 2 - // bit14:伺服故障代码 3 - // bit15:回零完成 - // bit16:编码器 C 脉冲有效 - // bit17:电机励磁 - // bit18:超程状态 - // bit19 编码器电池警告 - // bit20:编码器电池报数 - typedef struct { - uint32_t servo_is_ready : 1; - uint32_t servo_warning : 1; - uint32_t motor_is_rotating : 1; - uint32_t is_reach_target : 1; - uint32_t is_speed_consistent : 1; - uint32_t motor_is_zero_speed : 1; - uint32_t brake_is_on : 1; - uint32_t is_pos_close : 1; - uint32_t torque_is_limited : 1; - uint32_t speed_is_limited : 1; - uint32_t torque_is_detected : 1; - uint32_t servo_warning2 : 1; - uint32_t servo_error_code1 : 1; - uint32_t servo_error_code2 : 1; - uint32_t servo_error_code3 : 1; - uint32_t is_zero_complete : 1; - uint32_t encoder_c_pulse_is_valid : 1; - uint32_t motor_is_excited : 1; - uint32_t is_over_travel : 1; - uint32_t encoder_battery_warning : 1; - uint32_t encoder_battery_report : 1; - } status_bit; - private: ModbusBlockHost *m_modbusBlockHost; int32_t m_deviceId = 0; diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp index 8d39722..d9d3cc7 100644 --- a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp @@ -25,7 +25,29 @@ static void cmd_dump_ack(int32_t& ack) { // ZLOGI(TAG, "value:%d", ack); } static void cmd_dump_ack(Eq20ServoMotor::servo_internal_status_t ack) { // - ZLOGI(TAG, "value:%d", ack.status); + ZLOGI(TAG, "value:%d", ack.data.status); + ZLOGI(TAG, "servo_is_ready :%d", ack.data.sbit.servo_is_ready); + ZLOGI(TAG, "servo_warning :%d", ack.data.sbit.servo_warning); + ZLOGI(TAG, "motor_is_rotating :%d", ack.data.sbit.motor_is_rotating); + ZLOGI(TAG, "is_reach_target :%d", ack.data.sbit.is_reach_target); + ZLOGI(TAG, "is_speed_consistent :%d", ack.data.sbit.is_speed_consistent); + ZLOGI(TAG, "motor_is_zero_speed :%d", ack.data.sbit.motor_is_zero_speed); + ZLOGI(TAG, "brake_is_on :%d", ack.data.sbit.brake_is_on); + ZLOGI(TAG, "is_pos_close :%d", ack.data.sbit.is_pos_close); + ZLOGI(TAG, "torque_is_limited :%d", ack.data.sbit.torque_is_limited); + ZLOGI(TAG, "speed_is_limited :%d", ack.data.sbit.speed_is_limited); + ZLOGI(TAG, "torque_is_detected :%d", ack.data.sbit.torque_is_detected); + ZLOGI(TAG, "servo_warning2 :%d", ack.data.sbit.servo_warning2); + ZLOGI(TAG, "servo_error_code1 :%d", ack.data.sbit.servo_error_code1); + ZLOGI(TAG, "servo_error_code2 :%d", ack.data.sbit.servo_error_code2); + ZLOGI(TAG, "servo_error_code3 :%d", ack.data.sbit.servo_error_code3); + ZLOGI(TAG, "is_zero_complete :%d", ack.data.sbit.is_zero_complete); + ZLOGI(TAG, "encoder_c_pulse_is_valid:%d", ack.data.sbit.encoder_c_pulse_is_valid); + ZLOGI(TAG, "motor_is_excited :%d", ack.data.sbit.motor_is_excited); + ZLOGI(TAG, "is_over_travel :%d", ack.data.sbit.is_over_travel); + ZLOGI(TAG, "encoder_battery_warning:%d", ack.data.sbit.encoder_battery_warning); + ZLOGI(TAG, "encoder_battery_report :%d", ack.data.sbit.encoder_battery_report); + } static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } @@ -40,8 +62,8 @@ void ScriptCmderEq20Servomotor::regcmd() { REG_CMD___NO_ACK("eq20_", move_to, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); REG_CMD___NO_ACK("eq20_", move_by, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); REG_CMD___NO_ACK("eq20_", rotate, "(id,rpm,acctime)", 3, con->getInt(2), con->getInt(3)); - REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 5, con->getInt(2), con->getInt(3), con->getInt(4)); - REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 5, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); REG_CMD___NO_ACK("eq20_", stop, "(id)", 1); REG_CMD_WITH_ACK("eq20_", get_pos, "(id)", 1, int32_t, ack); REG_CMD_WITH_ACK("eq20_", get_servo_internal_state, "(id)", 1, Eq20ServoMotor::servo_internal_status_t, ack);