|
|
#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
|
