diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index 3bc5657..fc68b66 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -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, ®val, 1); } bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, ®val, 1); } bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)®val, 2); } diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index 537ee0f..7f0ef85 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/components/mini_servo_motor/feite_servo_motor.hpp @@ -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 \ No newline at end of file +} // namespace iflytop diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 1dc362a..4f4fa39 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -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 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 status_cb) { return 0; } int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function status_cb) { return 0; } int32_t MiniRobotCtrlModule::move_by(s32 pos, s32 speed, s32 torque, function status_cb) { return 0; } int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function status_cb) { return 0; } -int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, function 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 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; } diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index 3c3c0b5..4a004df 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -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: