You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
210 lines
7.5 KiB
210 lines
7.5 KiB
#pragma once
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#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
|