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
1db17e3903
commit
18d70a8b2e
4 changed files with 210 additions and 83 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. 132
      components/eq_20_asb_motor/eq20_servomotor.cpp
  2. 89
      components/eq_20_asb_motor/eq20_servomotor.hpp
  3. 48
      components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp
  4. 24
      components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp

132
components/eq_20_asb_motor/eq20_servomotor.cpp
View File

@ -35,13 +35,13 @@ write_pn 1 617 100 #Pn617
write_pn_bit 1 1501 0 0
write_pn_bit 1 1501 0 1
#endif
DO(write_pn(m_deviceId, 610, 0x0001));
DO(write_pn(m_deviceId, 615, rpm));
DO(write_pn(m_deviceId, 614, topos));
DO(write_pn(m_deviceId, 616, acctime));
DO(write_pn(m_deviceId, 617, acctime));
DO(write_pn_bit(m_deviceId, 1501, 0, 0));
DO(write_pn_bit(m_deviceId, 1501, 0, 1));
DO(write_pn(610, 0x0001));
DO(write_pn(615, rpm));
DO(write_pn(614, topos));
DO(write_pn(616, acctime));
DO(write_pn(617, acctime));
DO(write_pn_bit(1501, 0, 0));
DO(write_pn_bit(1501, 0, 1));
return 0;
}
int32_t Eq20ServoMotor::move_by(int32_t pos, int32_t rpm, int32_t acctime) {
@ -54,13 +54,13 @@ write_pn 1 617 100 #Pn617
write_pn_bit 1 1501 0 0
write_pn_bit 1 1501 0 1
#endif
DO(write_pn(m_deviceId, 610, 0x0011));
DO(write_pn(m_deviceId, 614, pos));
DO(write_pn(m_deviceId, 615, rpm));
DO(write_pn(m_deviceId, 616, acctime));
DO(write_pn(m_deviceId, 617, acctime));
DO(write_pn_bit(m_deviceId, 1501, 0, 0));
DO(write_pn_bit(m_deviceId, 1501, 0, 1));
DO(write_pn(610, 0x0011));
DO(write_pn(614, pos));
DO(write_pn(615, rpm));
DO(write_pn(616, acctime));
DO(write_pn(617, acctime));
DO(write_pn_bit(1501, 0, 0));
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); }
@ -77,46 +77,50 @@ write_pn 1 606 0 #
write_pn_bit 1 1501 1 0
write_pn_bit 1 1501 1 1
#endif
DO(write_pn(m_deviceId, 600, 11));
DO(write_pn(m_deviceId, 601, lookzeropoint_rpm));
DO(write_pn(m_deviceId, 602, findzero_edge_rpm));
DO(write_pn(m_deviceId, 603, lookzeropoint_acc_time));
DO(write_pn(m_deviceId, 604, 0));
DO(write_pn(m_deviceId, 605, 0));
DO(write_pn(m_deviceId, 606, 0));
DO(write_pn_bit(m_deviceId, 1501, 1, 0));
DO(write_pn_bit(m_deviceId, 1501, 1, 1));
DO(write_pn(600, 11));
DO(write_pn(601, lookzeropoint_rpm));
DO(write_pn(602, findzero_edge_rpm));
DO(write_pn(603, lookzeropoint_acc_time));
DO(write_pn(604, 0));
DO(write_pn(605, 0));
DO(write_pn(606, 0));
DO(write_pn_bit(1501, 1, 0));
DO(write_pn_bit(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(write_pn(m_deviceId, 600, 8));
DO(write_pn(m_deviceId, 601, lookzeropoint_rpm));
DO(write_pn(m_deviceId, 602, findzero_edge_rpm));
DO(write_pn(m_deviceId, 603, lookzeropoint_acc_time));
DO(write_pn(m_deviceId, 604, 0)); // 零点偏移
DO(write_pn(m_deviceId, 605, 0)); // 回零动作超时时间
DO(write_pn(m_deviceId, 606, 0)); // 回零动作中止使能
DO(write_pn_bit(m_deviceId, 1501, 1, 0));
DO(write_pn_bit(m_deviceId, 1501, 1, 1));
DO(write_pn(600, 8));
DO(write_pn(601, lookzeropoint_rpm));
DO(write_pn(602, findzero_edge_rpm));
DO(write_pn(603, lookzeropoint_acc_time));
DO(write_pn(604, 0)); // 零点偏移
DO(write_pn(605, 0)); // 回零动作超时时间
DO(write_pn(606, 0)); // 回零动作中止使能
DO(write_pn_bit(1501, 1, 0));
DO(write_pn_bit(1501, 1, 1));
return 0;
}
int32_t Eq20ServoMotor::getpos(int32_t &pos) {
DO(readreg(m_deviceId, 2036, pos));
int32_t Eq20ServoMotor::get_pos(int32_t &pos) {
DO(readreg(2036, pos));
return 0;
}
int32_t Eq20ServoMotor::enable(int32_t enable) {
DO(write_pn_bit(m_deviceId, 1501, 2, enable));
DO(write_pn_bit(1501, 2, enable));
return 0;
}
int32_t Eq20ServoMotor::stop() {
DO(write_pn_bit(m_deviceId, 1501, 0, 0));
DO(write_pn_bit(m_deviceId, 1501, 1, 0));
DO(write_pn_bit(1501, 0, 0));
DO(write_pn_bit(1501, 1, 0));
return 0;
}
int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) {
DO(read_pn(1065, state.status));
return 0;
}
int32_t Eq20ServoMotor::isReachTarget(uint8_t &isReachTarget) {}
#define AUTO_RESEND(exptr) \
int ret = 0; \
@ -127,57 +131,57 @@ int32_t Eq20ServoMotor::isReachTarget(uint8_t &isReachTarget) {}
} \
}
int32_t Eq20ServoMotor::writereg(int deviceid, uint32_t regadd, int32_t value) { //
AUTO_RESEND(_writereg(deviceid, regadd, value));
int32_t Eq20ServoMotor::writereg(uint32_t regadd, int32_t value) { //
AUTO_RESEND(_writereg(regadd, value));
}
int32_t Eq20ServoMotor::readreg(int deviceid, uint32_t regadd, int32_t &value) { //
AUTO_RESEND(_readreg(deviceid, regadd, value));
int32_t Eq20ServoMotor::readreg(uint32_t regadd, int32_t &value) { //
AUTO_RESEND(_readreg(regadd, value));
}
int32_t Eq20ServoMotor::write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value) { //
AUTO_RESEND(_write_reg_bit(deviceid, regadd, off, value));
int32_t Eq20ServoMotor::write_reg_bit(uint32_t regadd, int32_t off, int32_t value) { //
AUTO_RESEND(_write_reg_bit(regadd, off, value));
}
int32_t Eq20ServoMotor::write_pn(int deviceid, uint32_t pnadd, int32_t value) { //
AUTO_RESEND(_write_pn(deviceid, pnadd, value));
int32_t Eq20ServoMotor::write_pn(uint32_t pnadd, int32_t value) { //
AUTO_RESEND(_write_pn(pnadd, value));
}
int32_t Eq20ServoMotor::read_pn(int deviceid, uint32_t pnadd, int32_t &value) { //
AUTO_RESEND(_read_pn(deviceid, pnadd, value));
int32_t Eq20ServoMotor::read_pn(uint32_t pnadd, int32_t &value) { //
AUTO_RESEND(_read_pn(pnadd, value));
}
int32_t Eq20ServoMotor::write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value) { AUTO_RESEND(_write_pn_bit(deviceid, pnadd, off, value)); }
int32_t Eq20ServoMotor::read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_reg_bit(deviceid, regadd, off, value)); }
int32_t Eq20ServoMotor::read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_pn_bit(deviceid, pnadd, off, value)); }
int32_t Eq20ServoMotor::write_pn_bit(uint32_t pnadd, int32_t off, int32_t value) { AUTO_RESEND(_write_pn_bit(pnadd, off, value)); }
int32_t Eq20ServoMotor::read_reg_bit(uint32_t regadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_reg_bit(regadd, off, value)); }
int32_t Eq20ServoMotor::read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_pn_bit(pnadd, off, value)); }
/*******************************************************************************
* BASIC_API *
*******************************************************************************/
int32_t Eq20ServoMotor::_write_pn(int deviceid, uint32_t pnadd, int32_t value) { return _writereg(deviceid, pnadd * 2, value); }
int32_t Eq20ServoMotor::_read_pn(int deviceid, uint32_t pnadd, int32_t &value) { return _readreg(deviceid, pnadd * 2, value); }
int32_t Eq20ServoMotor::_write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value) { return _write_reg_bit(deviceid, pnadd * 2, off, value); }
int32_t Eq20ServoMotor::_read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value) { return _read_reg_bit(deviceid, pnadd * 2, off, value); }
int32_t Eq20ServoMotor::_write_pn(uint32_t pnadd, int32_t value) { return _writereg(pnadd * 2, value); }
int32_t Eq20ServoMotor::_read_pn(uint32_t pnadd, int32_t &value) { return _readreg(pnadd * 2, value); }
int32_t Eq20ServoMotor::_write_pn_bit(uint32_t pnadd, int32_t off, int32_t value) { return _write_reg_bit(pnadd * 2, off, value); }
int32_t Eq20ServoMotor::_read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value) { return _read_reg_bit(pnadd * 2, off, value); }
/*******************************************************************************
* REG *
*******************************************************************************/
int32_t Eq20ServoMotor::_writereg(int deviceid, uint32_t regadd, int32_t value) { //
DO(m_modbusBlockHost->writeReg10Muti(deviceid, regadd, (uint16_t *)&value, 2, 100));
int32_t Eq20ServoMotor::_writereg(uint32_t regadd, int32_t value) { //
DO(m_modbusBlockHost->writeReg10Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 100));
return 0;
}
int32_t Eq20ServoMotor::_readreg(int deviceid, uint32_t regadd, int32_t &value) { //
DO(m_modbusBlockHost->readReg03Muti(deviceid, regadd, (uint16_t *)&value, 2, 100));
int32_t Eq20ServoMotor::_readreg(uint32_t regadd, int32_t &value) { //
DO(m_modbusBlockHost->readReg03Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 100));
return 0;
}
int32_t Eq20ServoMotor::_write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value) {
int32_t Eq20ServoMotor::_write_reg_bit(uint32_t regadd, int32_t off, int32_t value) {
int32_t regval;
DO(_readreg(deviceid, regadd, regval));
DO(_readreg(regadd, regval));
if (value) {
regval |= (1 << off);
} else {
regval &= ~(1 << off);
}
DO(_writereg(deviceid, regadd, regval));
DO(_writereg(regadd, regval));
return 0;
}
int32_t Eq20ServoMotor::_read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value) {
int32_t Eq20ServoMotor::_read_reg_bit(uint32_t regadd, int32_t off, int32_t &value) {
int32_t regval;
DO(_readreg(deviceid, regadd, regval));
DO(_readreg(regadd, regval));
value = (regval >> off) & 0x01;
return 0;
}

89
components/eq_20_asb_motor/eq20_servomotor.hpp
View File

@ -9,6 +9,55 @@ using namespace std;
class Eq20ServoMotor {
public:
typedef struct {
int32_t status;
} 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;
@ -30,31 +79,33 @@ class Eq20ServoMotor {
int32_t move_to_zero_backward(int32_t lookzeropoint_rpm, int32_t findzero_edge_rpm, int32_t lookzeropoint_acc_time);
int32_t stop();
int32_t isReachTarget(uint8_t &isReachTarget);
// int32_t is_reach_target(int32_t &isReachTarget);
int32_t getpos(int32_t &pos);
int32_t get_servo_internal_state(servo_internal_status_t &state);
int32_t get_pos(int32_t &pos);
public:
int32_t writereg(int deviceid, uint32_t regadd, int32_t value);
int32_t readreg(int deviceid, uint32_t regadd, int32_t &value);
int32_t write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value);
int32_t read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value);
int32_t writereg(uint32_t regadd, int32_t value);
int32_t readreg(uint32_t regadd, int32_t &value);
int32_t write_reg_bit(uint32_t regadd, int32_t off, int32_t value);
int32_t read_reg_bit(uint32_t regadd, int32_t off, int32_t &value);
int32_t read_pn(int deviceid, uint32_t pnadd, int32_t &value);
int32_t write_pn(int deviceid, uint32_t pnadd, int32_t value);
int32_t write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value);
int32_t read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value);
int32_t read_pn(uint32_t pnadd, int32_t &value);
int32_t write_pn(uint32_t pnadd, int32_t value);
int32_t write_pn_bit(uint32_t pnadd, int32_t off, int32_t value);
int32_t read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value);
private:
int32_t _writereg(int deviceid, uint32_t regadd, int32_t value);
int32_t _readreg(int deviceid, uint32_t regadd, int32_t &value);
int32_t _write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value);
int32_t _read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value);
int32_t _write_pn(int deviceid, uint32_t pnadd, int32_t value);
int32_t _read_pn(int deviceid, uint32_t pnadd, int32_t &value);
int32_t _write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value);
int32_t _read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value);
int32_t _writereg(uint32_t regadd, int32_t value);
int32_t _readreg(uint32_t regadd, int32_t &value);
int32_t _write_reg_bit(uint32_t regadd, int32_t off, int32_t value);
int32_t _read_reg_bit(uint32_t regadd, int32_t off, int32_t &value);
int32_t _write_pn(uint32_t pnadd, int32_t value);
int32_t _read_pn(uint32_t pnadd, int32_t &value);
int32_t _write_pn_bit(uint32_t pnadd, int32_t off, int32_t value);
int32_t _read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value);
private:
int32_t send(uint8_t *data, size_t len, int32_t overtime);

48
components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp
View File

@ -0,0 +1,48 @@
#include "script_cmder_eq20_servomotor.hpp"
#include <string.h>
// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp"
using namespace iflytop;
#define TAG "CMD"
void ScriptCmderEq20Servomotor::initialize(CmdScheduler* cmdScheduler) {
m_cmdScheduler = cmdScheduler;
ZASSERT(m_cmdScheduler != nullptr);
regcmd();
}
int32_t ScriptCmderEq20Servomotor::findmodule(int id, Eq20ServoMotor** module) {
auto it = moduler.find(id);
if (it == moduler.end()) {
return err::kce_no_such_module;
}
*module = it->second;
return 0;
}
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);
}
static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) {
ZASSERT(moduler.find(id) == moduler.end());
ZASSERT(robot != nullptr);
moduler[id] = robot;
}
void ScriptCmderEq20Servomotor::regcmd() {
REG_CMD___NO_ACK("eq20_", enable, "(id,en)", 2, con->getBool(2));
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_", 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);
}

24
components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp
View File

@ -0,0 +1,24 @@
#pragma once
#include <map>
#include "eq20_servomotor.hpp"
#include "sdk/os/zos.hpp"
#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
namespace iflytop {
class ScriptCmderEq20Servomotor {
CmdScheduler* m_cmdScheduler;
map<uint8_t, Eq20ServoMotor*> moduler;
Eq20ServoMotor* module;
public:
void initialize(CmdScheduler* cmdScheduler);
void regmodule(int id, Eq20ServoMotor* robot);
void regcmd();
private:
int32_t findmodule(int id, Eq20ServoMotor** module);
};
} // namespace iflytop
Write Preview
Loading…
Cancel
Save