From ec71bb38f58311344cc8a5c113de910c064a0852 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 10 Oct 2023 15:57:48 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8E=BB=E6=8E=89=E9=83=A8=E5=88=86=E8=AD=A6?= =?UTF-8?q?=E5=91=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .settings/language.settings.xml | 4 +- sdk | 2 +- usrc/feite_servo_motor.cpp | 372 ---------------------------------------- usrc/feite_servo_motor.hpp | 210 +---------------------- 4 files changed, 4 insertions(+), 584 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 7f62e8b..08c9439 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/sdk b/sdk index 2b35686..d69f855 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 2b35686d8d692c80c80bdbb6b7e4afa38d94f860 +Subproject commit d69f8551a6307b256cb2917312e57e2949953db1 diff --git a/usrc/feite_servo_motor.cpp b/usrc/feite_servo_motor.cpp index 7171057..e69de29 100644 --- a/usrc/feite_servo_motor.cpp +++ b/usrc/feite_servo_motor.cpp @@ -1,372 +0,0 @@ -#include "feite_servo_motor.hpp" - -#include -#include - -// #include "board.h" -// #include "port.h" -using namespace iflytop; -using namespace std; -using namespace feite; -#define TAG "FeiTeServoMotor" -#define OVERTIME 30 -#define DO(func) \ - if (!(func)) { \ - ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \ - return false; \ - } - -static void dumphex(const char* tag, uint8_t* data, uint8_t len) { - #if 1 - printf("%s:", tag); - for (int i = 0; i < len; i++) { - printf("%02x ", data[i]); - } - printf("\n"); - #endif -} -void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) { - m_uart = uart; - m_hdma_rx = hdma_rx; - m_hdma_tx = hdma_tx; -} - -bool FeiTeServoMotor::ping(uint8_t id) { - ping_cmd_t ping_cmd; - ping_resp_t ping_resp; - ping_cmd.header = 0xffff; - ping_cmd.id = id; - ping_cmd.len = 2; - ping_cmd.cmd = kping; - ping_cmd.checksum = checksum_packet((uint8_t*)&ping_cmd, sizeof(ping_cmd_t)); - - return tx_and_rx((uint8_t*)&ping_cmd, sizeof(ping_cmd_t), (uint8_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME); -} - -static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) { - int16_t calibrate = nowpos - aftercalibratepos; - while (true) { - if (calibrate > 2047) { - calibrate -= 4094; - } else if (calibrate < -2047) { - calibrate += 4094; - } else { - break; - } - } - return calibrate; -} - -bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write_u8(id, kRegServoRunMode, (uint8_t)runmode); } -bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { return read_s16(id, kRegServoCalibration, 11, poscalibration); } - -run_mode_e FeiTeServoMotor::getmode(uint8_t id) { - uint8_t data = 0; - bool suc = read_u8(id, kRegServoRunMode, data); - if (suc) { - return (run_mode_e)data; - } else { - return kMotorMode; - } -} - -bool FeiTeServoMotor::getmode(uint8_t id, run_mode_e& runmode) { - uint8_t data = 0; - bool suc = read_u8(id, kRegServoRunMode, data); - runmode = (run_mode_e)data; - return suc; -} - -bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0); } -bool FeiTeServoMotor::getTorqueSwitch(uint8_t id, bool& on) { - uint8_t data = 0; - bool suc = read_u8(id, kRegServoTorqueSwitch, data); - on = data; - return suc; -} - -bool FeiTeServoMotor::getNowPos(uint8_t id, int16_t& pos) { return read_s16(id, kRegServoCurrentPos, 15, pos); } -bool FeiTeServoMotor::setTargetPos(uint8_t id, int16_t pos) { return write_s16(id, kRegServoTargetPos, 15, pos); } - -bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) { - cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; - cmd_header->header = 0xffff; - cmd_header->id = 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); - 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::getMoveFlag(uint8_t id, uint8_t& moveflag) { return read_u8(id, kRegServoMoveFlag, moveflag); } - -bool FeiTeServoMotor::reCalibration(int id, int16_t pos) { - if (pos < 0 || pos > 4095) { - ZLOGE(TAG, "reCalibration pos:%d out of range", pos); - return false; - } - /** - * @brief 关闭扭矩快关，防止舵机运动 - */ - setTorqueSwitch(id, false); - /** - * @brief 设置当前模式为位置模式 - */ - DO(setmode(id, kServoMode)); - - int16_t curpos; - DO(getNowPos(id, curpos)); - int16_t curcalibrate; - DO(getServoCalibration(id, curcalibrate)); - int16_t realpos = curpos + curcalibrate; - int16_t newcalibrate = getcalibrate(realpos, pos); - - ZLOGI(TAG, "reCalibration id:%d curpos:%d curcalibrate:%d realpos:%d newcalibrate:%d", id, curpos, curcalibrate, realpos, newcalibrate); - /** - * @brief 写入新的校准值 - */ - DO(write_u8(id, kRegServoLockFlag, 0)); - DO(write_s16(id, kRegServoCalibration, 11, newcalibrate)); - DO(write_u8(id, kRegServoLockFlag, 1)); - /** - * @brief 重新设置目标位置为当前这个位置 - */ - int16_t nowpos; - DO(getNowPos(id, nowpos)); - ZLOGI(TAG, "reCalibration id:%d nowpos:%d:%d", id, nowpos, pos); - DO(setTargetPos(id, pos)); - return true; -} - -/******************************************************************************* - * 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); } -bool FeiTeServoMotor::read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { return read_reg(id, add, (uint8_t*)®val, 2); } -bool FeiTeServoMotor::async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, true, add, ®val, 1); } -bool FeiTeServoMotor::async_write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, true, add, (uint8_t*)®val, 2); } -bool FeiTeServoMotor::async_write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) { - uint16_t val = 0; - if (regval >= 0) { - val = regval; - } else { - val = -regval; - val |= (1 << signbitoff); - } - return async_write_u16(id, add, val); -} - -bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval) { - uint16_t val = 0; - bool ret = read_u16(id, add, val); - if (!ret) return false; - uint8_t sign = (val >> signbitoff) & 0x01; - uint16_t realval = val & (~(1 << signbitoff)); - if (sign == 0) { - regval = realval; - } else { - regval = -realval; - } - return true; -} -bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) { - uint16_t val = 0; - if (regval >= 0) { - val = regval; - } else { - val = -regval; - val |= (1 << signbitoff); - } - return write_u16(id, add, val); -} -bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // - // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); - cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; - receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; - cmd_header->header = 0xffff; - cmd_header->id = id; - cmd_header->len = 3 + len; // 3 == cmd + add + checksum - cmd_header->cmd = async ? kasyncWrite : kwrite; - cmd_header->data[0] = add; - memcpy(&cmd_header->data[1], data, len); - - int txpacketlen = sizeof(cmd_header_t) + 1 + len + 1; - int rxpacketlen = sizeof(receipt_header_t) + 1; - - uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); - m_txbuf[txpacketlen - 1] = checksum; - if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { - ZLOGE(TAG, "write_reg fail,overtime"); - return false; - } - if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { - ZLOGE(TAG, "write_reg fail,receipt header error"); - return false; - } - return true; -} -bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { - // return false; - cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; - receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; - cmd_header->header = 0xffff; - cmd_header->id = id; - cmd_header->len = 3 + 1; // 4 == cmd + add + checksum + readlen - cmd_header->cmd = kread; - cmd_header->data[0] = add; - cmd_header->data[1] = len; - - int txpacketlen = sizeof(cmd_header_t) + 3; - int rxpacketlen = sizeof(receipt_header_t) + 1 + len; - - uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); - - m_txbuf[txpacketlen - 1] = checksum; - if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { - return false; - } - if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { - ZLOGE(TAG, "read_reg fail,receipt header error"); - return false; - } - memcpy(data, receipt_header->data, len); - return true; -} -bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems) { - uint32_t enter_ticket = HAL_GetTick(); - dumphex("tx:", tx, txdatalen); - - HAL_UART_Transmit(m_uart, tx, txdatalen, 1000); - HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize); - - - bool overtime_flag = false; - while (HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_RX || // - HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_TX_RX) { - osDelay(1); - int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx); - if (rxsize == expectrxsize) { - dumphex("rx:", rx, expectrxsize); - break; - } - if (zos_haspassedms(enter_ticket) > overtimems) { - if (expectrxsize != 0 && rxsize != 0) { - ZLOGW(TAG, "txandrx overtime rxsize:%d != expect_size:%d", rxsize, expectrxsize); - } - overtime_flag = true; - break; - } - } - HAL_UART_DMAStop(m_uart); - if (overtime_flag) { - return false; - } - return true; -} - -bool FeiTeServoMotor::readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion) { - uint8_t data = 0; - DO(read_u8(id, kRegFirmwareMainVersion, data)); - mainversion = data; - DO(read_u8(id, kRegFirmwareSubVersion, data)); - subversion = data; - DO(read_u8(id, kRegServoMainVersion, data)); - miniserv_mainversion = data; - DO(read_u8(id, kRegServoSubVersion, data)); - miniserv_subversion = data; - return true; -} - -uint8_t FeiTeServoMotor::checksum_packet(uint8_t* data, uint8_t len) { return checksum(&data[2], len - 3); } -uint8_t FeiTeServoMotor::checksum(uint8_t* data, uint8_t len) { - // CheckSum=~(ID+Length+Instruction+Parameter1+...ParameterN - uint16_t sum = 0; - for (int i = 0; i < len; i++) { - sum += data[i]; - } - return ~(sum & 0xff); -} diff --git a/usrc/feite_servo_motor.hpp b/usrc/feite_servo_motor.hpp index a308e5c..f3bb450 100644 --- a/usrc/feite_servo_motor.hpp +++ b/usrc/feite_servo_motor.hpp @@ -1,210 +1,2 @@ #pragma once -#include -#include -#include -#include -#include "sdk/os/zos.hpp" - -// #include "sdk/os/zos.hpp" - -#define ZSTRUCT(name, ...) \ - typedef struct { \ - __VA_ARGS__ \ - } name; - -namespace iflytop { -namespace feite { - -typedef enum { - kping = 0x01, - kread = 0x02, - kwrite = 0x03, - kasyncWrite = 0x04, -} cmd_e; - -typedef enum { - kServoMode = 0, // 位置伺服模式 42 号地址来控制电机位置 - kMotorMode = 1, // 电机恒速模式 46 号地址运行速度来控制电机速度 BIT15 为方向位 - kOpenMotorMode = 2, // 扭矩电机模式 44 号地址来控制，1000满扭矩 - kStepMotorMode = 3, // 步进电机模式 -} run_mode_e; // reg:33 - -typedef enum { - kRegFirmwareMainVersion = 0, // 固件主版本号 - kRegFirmwareSubVersion = 1, // 固件次版本号 - kRegServoMainVersion = 3, // 舵机主版本号 - kRegServoSubVersion = 4, // 舵机次版本号 - kRegServoId = 5, // ID - kRegServoBaudRate = 6, // 波特率 - kRegServoDelay = 7, // 返回延时 - kRegServoAckLevel = 8, // 应答状态级别 - kRegServoMinAngle = 9, // 最小角度限制 - kRegServoMaxAngle = 11, // 最大角度限制 - kRegServoMaxTemp = 13, // 最高温度上限 - kRegServoMaxVoltage = 14, // 最高输入电压 - kRegServoMinVoltage = 15, // 最低输入电压 - kRegServoMaxTorque = 16, // 最大扭矩 - kRegServoPhase = 18, // 相位 - kRegServoUnloadCondition = 19, // 卸载条件 - kRegServoLedAlarmCondition = 20, // LED 报警条件 - kRegServoP = 21, // P 比例系 - kRegServoD = 22, // D 微分系 - kRegServoI = 23, // I - kRegServoMinStart = 24, // 最小启动 - kRegServoCwDeadZone = 26, // 顺时针不灵敏区 - kRegServoCcwDeadZone = 27, // 逆时针不灵敏 - kRegServoProtectCurrent = 28, // 保护电流 - kRegServoAngleResolution = 30, // 角度分辨 - - kRegServoCalibration = 31, // 位置校正 BIT11为方向位，表示正负方向，BIT0~10位表示范围0-2047步 - kRegServoRunMode = 33, // 运行模式 - kRegServoProtectTorque = 34, // 保护扭矩 - kRegServoProtectTime = 35, // 保护时间 - kRegServoOverloadTorque = 36, // 过载扭矩 - kRegServoSpeedP = 37, // 速度闭环P比例参数 - kRegServoOverloadTime = 38, // 过流保护时间 - kRegServoSpeedI = 39, // 速度闭环I积分参数 - kRegServoTorqueSwitch = 40, // 扭矩开关 - kRegServoAcc = 41, // 加速度 - kRegServoTargetPos = 42, // 目标位置 - kRegServoRunTime = 44, // 运行时间 - kRegServoRunSpeed = 46, // 运行速度 - kRegServoTorqueLimit = 48, // 转矩限制 - kRegServoLockFlag = 55, // 锁标志 - kRegServoCurrentPos = 56, // 当前位置 - kRegServoCurrentSpeed = 58, // 当前速度 - kRegServoCurrentLoad = 60, // 当前负载 bit10为方向位 - kRegServoCurrentVoltage = 62, // 当前电压 - kRegServoCurrentTemp = 63, // 当前温度 - kRegServoAsyncWriteFlag = 64, // 异步写标志 - kRegServoStatus = 65, // 舵机状态 - kRegServoMoveFlag = 66, // 移动标志 - kRegServoCurrentCurrent = 69, // 当前电流 - - kRegServoCheckSpeed = 80, // 80 移动检查速度 - kRegServoDTime = 81, // 81 D控制时间 - kRegServoSpeedUnit = 82, // 82 速度单位系数 - kRegServoMinSpeedLimit = 83, // 83 最小速度限制 - kRegServoMaxSpeedLimit = 84, // 84 最大速度限制 - kRegServoAccLimit = 85, // 85 加速度限制 - kRegServoAccMultiple = 86, // 86 加速度倍数 -} reg_add_e; -#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;) -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[];) - -#pragma pack() - -}; // namespace feite - -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; - - uint8_t m_txbuf[128] = {0}; - uint8_t m_rxbuf[128] = {0}; - - public: - void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); - bool ping(uint8_t id); - bool readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion); - - bool setmode(uint8_t id, run_mode_e runmode); - run_mode_e getmode(uint8_t id); - bool getmode(uint8_t id, run_mode_e& runmode); - - // kRegServoTorqueSwitch - bool setTorqueSwitch(uint8_t id, bool on); - bool getTorqueSwitch(uint8_t id, bool& on); - - bool getNowPos(uint8_t id, int16_t& pos); - bool setTargetPos(uint8_t id, int16_t pos); - - bool getServoCalibration(uint8_t, int16_t& poscalibration); - bool reCalibration(int id, int16_t pos); - - 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); - - bool getMoveFlag(uint8_t id, uint8_t& moveflag); - - 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); - bool write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval); - - bool async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval); - bool async_write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval); - bool async_write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval); - - bool read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval); - bool read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval); - bool read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval); - - public: - bool write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len); - bool read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len); - - bool tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems); - private: - - uint8_t checksum(uint8_t* data, uint8_t len); - uint8_t checksum_packet(uint8_t* data, uint8_t len); -}; -} // namespace iflytop +#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp" \ No newline at end of file