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
541da22a0f
commit
031f4de05c
3 changed files with 85 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      components/eq_20_asb_motor/eq20_servomotor.cpp
  2. 94
      components/eq_20_asb_motor/eq20_servomotor.hpp
  3. 28
      components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp

16
components/eq_20_asb_motor/eq20_servomotor.cpp
View File

@ -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;
}

94
components/eq_20_asb_motor/eq20_servomotor.hpp
View File

@ -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;

28
components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp
View File

@ -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);
Write Preview
Loading…
Cancel
Save