#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