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
6cea5addaf
commit
3b18045188
4 changed files with 175 additions and 32 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. 108
      components/mini_servo_motor/feite_servo_motor.cpp
  2. 68
      components/mini_servo_motor/feite_servo_motor.hpp
  3. 29
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
  4. 2
      components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

108
components/mini_servo_motor/feite_servo_motor.cpp
View File

@ -12,6 +12,14 @@ using namespace feite;
ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \
return false; \
}
static void dumphex(char* tag, uint8_t* data, uint8_t len) {
printf("%s:", tag);
for (int i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) {
m_uart = uart;
m_hdma_rx = hdma_rx;
@ -81,9 +89,91 @@ bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) {
cmd_header->len = 2;
cmd_header->cmd = 5;
cmd_header->data[0] = checksum((uint8_t*)cmd_header, sizeof(cmd_header_t) + 1);
HAL_UART_Transmit(m_uart, m_txbuf, sizeof(cmd_header_t) + 1, 1000);
// HAL_UART_Transmit(m_uart, m_txbuf, sizeof(cmd_header_t) + 1, 1000);
return true;
}
bool FeiTeServoMotor::rotate(uint8_t id, int16_t speed, uint16_t torque) {
DO(setmode(id, kMotorMode));
if (torque == 0) torque = 1000;
DO(write_u16(id, kRegServoTorqueLimit, torque));
DO(write_s16(id, kRegServoRunSpeed, 15, speed));
return true;
}
bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t torque) {
/**
* @brief ÉèÖÃŤ¾Ø
*/
DO(setmode(id, kServoMode));
if (torque == 0) torque = 1000;
DO(write_u16(id, kRegServoTorqueLimit, torque));
DO(write_s16(id, kRegServoRunSpeed, 15, speed));
DO(setTargetPos(id, pos));
return true;
}
uint16_t abs16(int16_t val) {
if (val < 0) {
return -val;
} else {
return val;
}
}
bool FeiTeServoMotor::moveWithTorque(uint8_t id, int16_t torque) {
DO(setmode(id, kOpenMotorMode));
if (torque == 0) torque = 1000;
DO(write_u16(id, kRegServoTorqueLimit, abs16(torque)));
DO(write_s16(id, kRegServoRunTime, 15, torque));
return true;
}
static int16_t tosign16(uint16_t* d, int signoff) {
uint16_t sign = (*d >> signoff) & 0x01;
uint16_t val = *d & (~(1 << signoff));
if (sign == 0) {
return val;
} else {
return -val;
}
}
bool FeiTeServoMotor::read_status(uint8_t id, status_t* status) {
// kRegServoCurrentPos
bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)status, sizeof(status_t));
status->vel = tosign16((uint16_t*)&status->vel, 15);
if (!suc) return false;
return true;
}
bool FeiTeServoMotor::read_detailed_status(uint8_t id, detailed_status_t* detailed_status) {
bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)detailed_status, sizeof(*detailed_status));
if (!suc) return false;
detailed_status->vel = tosign16((uint16_t*)&detailed_status->vel, 15);
detailed_status->torque = tosign16((uint16_t*)&detailed_status->torque, 10);
return true;
}
void FeiTeServoMotor::dump_status(status_t* status) {
ZLOGI(TAG, "===========status===========");
ZLOGI(TAG, "= status->pos :%d", status->pos);
ZLOGI(TAG, "= status->vel :%d", status->vel);
ZLOGI(TAG, "= status->torque :%d", status->torque);
ZLOGI(TAG, "=");
}
#define BIT_IN_BYTE(byte, off) ((byte >> off) & 0x01)
void FeiTeServoMotor::dump_detailed_status(detailed_status_t* detailed_status) {
ZLOGI(TAG, "===========detailed_status===========");
ZLOGI(TAG, "= detailed_status->pos :%d", detailed_status->pos);
ZLOGI(TAG, "= detailed_status->vel :%d", detailed_status->vel);
ZLOGI(TAG, "= detailed_status->torque :%d", detailed_status->torque);
ZLOGI(TAG, "= detailed_status->voltage :%d", detailed_status->voltage);
ZLOGI(TAG, "= detailed_status->temperature:%d", detailed_status->temperature);
ZLOGI(TAG, "= detailed_status->state :%d:%d:%d:%d:%d:%d", BIT_IN_BYTE(detailed_status->state, 0), BIT_IN_BYTE(detailed_status->state, 1), BIT_IN_BYTE(detailed_status->state, 2), BIT_IN_BYTE(detailed_status->state, 3), BIT_IN_BYTE(detailed_status->state, 4),
BIT_IN_BYTE(detailed_status->state, 5));
ZLOGI(TAG, "= detailed_status->moveflag :%d", detailed_status->moveflag);
ZLOGI(TAG, "= detailed_status->current :%d", detailed_status->current);
ZLOGI(TAG, "=");
}
bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
if (pos < 0 || pos > 4095) {
@ -123,19 +213,9 @@ bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
return true;
}
#define DO(func) \
if (!(func)) { \
ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \
return false; \
}
static void dumphex(char* tag, uint8_t* data, uint8_t len) {
printf("%s:", tag);
for (int i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
/*******************************************************************************
* BASEFUNC *
*******************************************************************************/
bool FeiTeServoMotor::write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, false, add, &regval, 1); }
bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, &regval, 1); }
bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)&regval, 2); }

68
components/mini_servo_motor/feite_servo_motor.hpp
View File

@ -66,7 +66,7 @@ typedef enum {
kRegServoLockFlag = 55, // 锁标志
kRegServoCurrentPos = 56, // 当前位置
kRegServoCurrentSpeed = 58, // 当前速度
kRegServoCurrentLoad = 60, // 当前负载
kRegServoCurrentLoad = 60, // 当前负载 bit10为方向位
kRegServoCurrentVoltage = 62, // 当前电压
kRegServoCurrentTemp = 63, // 当前温度
kRegServoAsyncWriteFlag = 64, // 异步写标志
@ -85,17 +85,6 @@ typedef enum {
#pragma pack(1)
ZSTRUCT(ping_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t checksum;)
ZSTRUCT(ping_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;)
#if 0
ZSTRUCT(read_u8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;)
ZSTRUCT(read_u8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t data; uint8_t checksum;)
ZSTRUCT(write_u8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t regval; uint8_t checksum;)
ZSTRUCT(write_u8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;)
ZSTRUCT(read_u16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;)
ZSTRUCT(read_u16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint16_t data; uint8_t checksum;)
ZSTRUCT(write_u16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint16_t regval; uint8_t checksum;)
ZSTRUCT(write_u16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;)
#endif
ZSTRUCT(receipt_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t data[];)
ZSTRUCT(cmd_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t data[];)
@ -105,6 +94,30 @@ ZSTRUCT(cmd_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint
using namespace feite;
class FeiTeServoMotor {
public:
#pragma pack(1)
typedef struct {
uint16_t pos; // 56 当前位置 (0->4096)
int16_t vel; // 58 有符号 bit15为方向位 0.732RPM
int16_t torque; // 60 扭矩:0.1%
} status_t;
typedef struct {
uint16_t pos; // 56 当前位置 (0->4096)
int16_t vel; // 58 有符号 bit15为方向位 0.732RPM
int16_t torque; // 60 有符号 bit10为方向位 扭矩:0.1%
uint8_t voltage; // 62 电压:0.1v
uint8_t temperature; // 63 温度:1度
uint8_t __pad0; // 64 占位
uint8_t state; // 65 舵机状态 Bit0:电压 Bit1:传感器 Bit2:温度 Bit3:电流 Bit4:角度 Bit5:过载
uint8_t moveflag; // 66 移动标志
uint8_t __pad1; // 67
uint8_t __pad2; // 68
uint16_t current; // 69 当前电流 单位:6.5mA
} detailed_status_t;
#pragma pack()
private:
UART_HandleTypeDef* m_uart;
DMA_HandleTypeDef* m_hdma_rx;
DMA_HandleTypeDef* m_hdma_tx;
@ -133,6 +146,35 @@ class FeiTeServoMotor {
bool triggerAysncWrite(uint8_t id);
/**
* @brief 旋转
*
* @param id
* @param speed 运行速度
* @param torque 扭矩限制 0->1000 0/1000为最大扭矩
* @return true
* @return false
*/
bool rotate(uint8_t id, int16_t speed, uint16_t torque = 0);
/**
* @brief 移动到指定位置
*
* @param id
* @param pos
* @param speed
* @param torque
* @return true
* @return false
*/
bool moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t torque);
bool moveWithTorque(uint8_t id, int16_t torque);
bool read_status(uint8_t id, status_t* status);
void dump_status(status_t* status);
bool read_detailed_status(uint8_t id, detailed_status_t* detailed_status);
void dump_detailed_status(detailed_status_t* detailed_status);
public:
bool write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval);
bool write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval);
@ -155,4 +197,4 @@ class FeiTeServoMotor {
uint8_t checksum(uint8_t* data, uint8_t len);
uint8_t checksum_packet(uint8_t* data, uint8_t len);
};
} // namespace iflytop
} // namespace iflytop

29
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -26,17 +26,38 @@ int32_t MiniRobotCtrlModule::stop(u8 stop_type) {
return err::ksucc;
}
int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) {
if (nowpos < 0 || nowpos > 4095) return err::kcommon_error_param_out_of_range;
if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime;
return 0;
}
int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) {
/**
* @brief
* 1:切换成电机模式
* 2:写入速度
* 3:启动线程，线程退后，设置速度为0
*/
m_thread.stop();
if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
return 0;
}
int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) { return 0; }
int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) { return 0; }
int32_t MiniRobotCtrlModule::move_by(s32 pos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) { return 0; }
int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) { return 0; }
int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, function<void(move_by_nolimit_cb_status_t& status)> status_cb) { return 0; }
int32_t MiniRobotCtrlModule::read_version(version_t& version) { return 0; }
int32_t MiniRobotCtrlModule::read_status(status_t& status) { return 0; }
int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, function<void(move_by_nolimit_cb_status_t& status)> status_cb) { return err::kcommon_error_operation_not_support; }
int32_t MiniRobotCtrlModule::read_version(version_t& version) {
uint8_t mainversion, subversion, miniserv_mainversion, miniserv_subversion;
if (!m_bus->readversion(m_id, mainversion, subversion, miniserv_mainversion, miniserv_subversion)) return err::kce_subdevice_overtime;
version.version = mainversion << 24 | subversion << 16 | miniserv_mainversion << 8 | miniserv_subversion;
return 0;
}
int32_t MiniRobotCtrlModule::read_status(status_t& status) {
return 0; }
int32_t MiniRobotCtrlModule::read_debug_info(debug_info_t& debug_info) { return 0; }
int32_t MiniRobotCtrlModule::set_run_param(run_param_t& param) { return 0; }

2
components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
View File

@ -9,7 +9,7 @@ namespace iflytop {
class MiniRobotCtrlModule : public I_MiniServoModule {
FeiTeServoMotor* m_bus;
uint8_t m_id;
ZThread m_thread;
s32 m_pos_shift;
public:

Write Preview
Loading…
Cancel
Save