6 changed files with 485 additions and 5 deletions
-
11components/errorcode/errorcode.hpp
-
252components/mini_servo_motor/feite_servo_motor.cpp
-
134components/mini_servo_motor/feite_servo_motor.hpp
-
48components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
-
40components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp
-
5os/zos.hpp
@ -0,0 +1,252 @@ |
|||||
|
#include "feite_servo_motor.hpp"
|
||||
|
using namespace iflytop; |
||||
|
using namespace std; |
||||
|
using namespace feite; |
||||
|
#define TAG "FeiTeServoMotor"
|
||||
|
#define OVERTIME 5
|
||||
|
#define DO(func) \
|
||||
|
if (!(func)) { \ |
||||
|
ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \ |
||||
|
return false; \ |
||||
|
} |
||||
|
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); |
||||
|
} |
||||
|
bool FeiTeServoMotor::write8(uint8_t id, feite::reg_add_e add, uint8_t regval) { |
||||
|
write8_cmd_t write8_cmd = {0}; |
||||
|
write8_resp_t write8_resp = {0}; |
||||
|
write8_cmd.header = 0xffff; |
||||
|
write8_cmd.id = id; |
||||
|
write8_cmd.len = 3 + 1; |
||||
|
write8_cmd.cmd = kwrite; |
||||
|
write8_cmd.regadd = add; |
||||
|
write8_cmd.regval = regval; |
||||
|
write8_cmd.checksum = checksum_packet((uint8_t*)&write8_cmd, sizeof(write8_cmd_t)); |
||||
|
return tx_and_rx((uint8_t*)&write8_cmd, sizeof(write8_cmd_t), (uint8_t*)&write8_resp, sizeof(write8_resp_t), OVERTIME); |
||||
|
} |
||||
|
bool FeiTeServoMotor::read8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { |
||||
|
read8_cmd_t read8_cmd = {0}; |
||||
|
read8_resp_t read8_resp = {0}; |
||||
|
read8_cmd.header = 0xffff; |
||||
|
read8_cmd.id = id; |
||||
|
read8_cmd.len = 3 + 1; |
||||
|
read8_cmd.cmd = kread; |
||||
|
read8_cmd.regadd = add; |
||||
|
read8_cmd.readlen = 1; |
||||
|
read8_cmd.checksum = checksum_packet((uint8_t*)&read8_cmd, sizeof(read8_cmd_t)); |
||||
|
if (!tx_and_rx((uint8_t*)&read8_cmd, sizeof(read8_cmd_t), (uint8_t*)&read8_resp, sizeof(read8_resp_t), OVERTIME)) { |
||||
|
return false; |
||||
|
} |
||||
|
regval = read8_resp.data; |
||||
|
return true; |
||||
|
} |
||||
|
|
||||
|
bool FeiTeServoMotor::write16(uint8_t id, feite::reg_add_e add, uint16_t regval) { |
||||
|
write16_cmd_t write16_cmd = {0}; |
||||
|
write16_resp_t write16_resp = {0}; |
||||
|
write16_cmd.header = 0xffff; |
||||
|
write16_cmd.id = id; |
||||
|
write16_cmd.len = 3 + 2; |
||||
|
write16_cmd.cmd = kwrite; |
||||
|
write16_cmd.regadd = add; |
||||
|
write16_cmd.regval = regval; |
||||
|
write16_cmd.checksum = checksum_packet((uint8_t*)&write16_cmd, sizeof(write16_cmd_t)); |
||||
|
return tx_and_rx((uint8_t*)&write16_cmd, sizeof(write16_cmd_t), (uint8_t*)&write16_resp, sizeof(write16_resp_t), OVERTIME); |
||||
|
} |
||||
|
|
||||
|
bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, int16_t& regval) { |
||||
|
uint16_t val = 0; |
||||
|
bool ret = read16(id, add, val); |
||||
|
if (!ret) return false; |
||||
|
|
||||
|
uint8_t sign = val >> 15; |
||||
|
uint16_t realval = val & 0x7fff; |
||||
|
if (sign == 0) { |
||||
|
regval = realval; |
||||
|
} else { |
||||
|
regval = -realval; |
||||
|
} |
||||
|
return true; |
||||
|
} |
||||
|
|
||||
|
bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, int16_t regval) { |
||||
|
uint16_t val = 0; |
||||
|
if (regval >= 0) { |
||||
|
val = regval; |
||||
|
} else { |
||||
|
val = -regval; |
||||
|
val |= 0x8000; |
||||
|
} |
||||
|
return write16(id, add, val); |
||||
|
} |
||||
|
|
||||
|
bool FeiTeServoMotor::read16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { |
||||
|
read16_cmd_t read16_cmd = {0}; |
||||
|
read16_resp_t read16_resp = {0}; |
||||
|
read16_cmd.header = 0xffff; |
||||
|
read16_cmd.id = id; |
||||
|
read16_cmd.len = 3 + 1; |
||||
|
read16_cmd.cmd = kread; |
||||
|
read16_cmd.regadd = add; |
||||
|
read16_cmd.readlen = 2; |
||||
|
read16_cmd.checksum = checksum_packet((uint8_t*)&read16_cmd, sizeof(read16_cmd_t)); |
||||
|
if (!tx_and_rx((uint8_t*)&read16_cmd, sizeof(read16_cmd_t), (uint8_t*)&read16_resp, sizeof(read16_resp_t), OVERTIME)) { |
||||
|
return false; |
||||
|
} |
||||
|
regval = read16_resp.data; |
||||
|
return true; |
||||
|
} |
||||
|
|
||||
|
bool FeiTeServoMotor::async_write8(uint8_t id, feite::reg_add_e add, uint8_t regval) { |
||||
|
write8_cmd_t write8_cmd = {0}; |
||||
|
write8_resp_t write8_resp = {0}; |
||||
|
write8_cmd.header = 0xffff; |
||||
|
write8_cmd.id = id; |
||||
|
write8_cmd.len = 3 + 1; |
||||
|
write8_cmd.cmd = kasyncWrite; |
||||
|
write8_cmd.regadd = add; |
||||
|
write8_cmd.regval = regval; |
||||
|
write8_cmd.checksum = checksum_packet((uint8_t*)&write8_cmd, sizeof(write8_cmd_t)); |
||||
|
return tx_and_rx((uint8_t*)&write8_cmd, sizeof(write8_cmd_t), (uint8_t*)&write8_resp, sizeof(write8_resp_t), OVERTIME); |
||||
|
} |
||||
|
bool FeiTeServoMotor::async_write16(uint8_t id, feite::reg_add_e add, uint16_t regval) { |
||||
|
write16_cmd_t write16_cmd = {0}; |
||||
|
write16_resp_t write16_resp = {0}; |
||||
|
write16_cmd.header = 0xffff; |
||||
|
write16_cmd.id = id; |
||||
|
write16_cmd.len = 3 + 2; |
||||
|
write16_cmd.cmd = kasyncWrite; |
||||
|
write16_cmd.regadd = add; |
||||
|
write16_cmd.regval = regval; |
||||
|
write16_cmd.checksum = checksum_packet((uint8_t*)&write16_cmd, sizeof(write16_cmd_t)); |
||||
|
return tx_and_rx((uint8_t*)&write16_cmd, sizeof(write16_cmd_t), (uint8_t*)&write16_resp, sizeof(write16_resp_t), OVERTIME); |
||||
|
} |
||||
|
|
||||
|
static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) { |
||||
|
int16_t calibrate = aftercalibratepos - nowpos; |
||||
|
while (true) { |
||||
|
if (calibrate > 2047) { |
||||
|
calibrate -= 4094; |
||||
|
} else if (calibrate < -2047) { |
||||
|
calibrate += 4094; |
||||
|
} else { |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
return calibrate; |
||||
|
} |
||||
|
|
||||
|
bool FeiTeServoMotor::setcurpos(int id, int16_t pos) { |
||||
|
/**
|
||||
|
* @brief |
||||
|
* 1. 记录当前模式 |
||||
|
* 2. 切换到位置模式 |
||||
|
* 3. 读取当前位置 |
||||
|
* 4. 读取当前位置校准数值 |
||||
|
* 5. 计算新的校准值 |
||||
|
* 6. 切换到速度模式（速度模式下不会因为位置的变化而导致舵机运动） |
||||
|
* 解锁写入 |
||||
|
* 6. 写入新的校准值 |
||||
|
* 锁定写入 |
||||
|
* 7. 切换回原来的模式 |
||||
|
*/ |
||||
|
if (pos < 0 || pos > 4095) { |
||||
|
ZLOGE(TAG, "setcurpos pos:%d out of range", pos); |
||||
|
return false; |
||||
|
} |
||||
|
|
||||
|
run_mode_e nowmode = getmode(id); |
||||
|
DO(setmode(id, kServoMode)); |
||||
|
uint16_t curpos; |
||||
|
DO(read16(id, kRegServoCurrentPos, curpos)); |
||||
|
int16_t curcalibrate; |
||||
|
DO(read_s16(id, kRegServoCalibration, curcalibrate)); |
||||
|
|
||||
|
int16_t realpos = curpos - curcalibrate; |
||||
|
int16_t newcalibrate = getcalibrate(realpos, pos); |
||||
|
|
||||
|
DO(setmode(id, kMotorMode)); |
||||
|
DO(write8(id, kRegServoLockFlag, 0)); |
||||
|
DO(write_s16(id, kRegServoCalibration, newcalibrate)); |
||||
|
DO(write8(id, kRegServoLockFlag, 1)); |
||||
|
|
||||
|
DO(setmode(id, nowmode)); |
||||
|
return true; |
||||
|
} |
||||
|
|
||||
|
#define DO(func) \
|
||||
|
if (!(func)) { \ |
||||
|
ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \ |
||||
|
return false; \ |
||||
|
} |
||||
|
|
||||
|
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(); |
||||
|
|
||||
|
HAL_UART_Transmit_DMA(m_uart, tx, txdatalen); |
||||
|
while (HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_TX) { |
||||
|
; |
||||
|
} |
||||
|
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(5); |
||||
|
int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx); |
||||
|
if (rxsize == 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(read8(id, kRegFirmwareMainVersion, data)); |
||||
|
mainversion = data; |
||||
|
DO(read8(id, kRegFirmwareSubVersion, data)); |
||||
|
subversion = data; |
||||
|
DO(read8(id, kRegServoMainVersion, data)); |
||||
|
miniserv_mainversion = data; |
||||
|
DO(read8(id, kRegServoSubVersion, data)); |
||||
|
miniserv_subversion = data; |
||||
|
return true; |
||||
|
} |
||||
|
bool FeiTeServoMotor::readposcalibration(uint8_t id, int16_t& poscalibration) { |
||||
|
return read_s16(id, kRegServoCalibration, poscalibration); |
||||
|
} |
||||
|
|
||||
|
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); |
||||
|
} |
||||
@ -0,0 +1,134 @@ |
|||||
|
#pragma once
|
||||
|
#include <stdbool.h>
|
||||
|
#include <stdint.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include <stdlib.h>
|
||||
|
|
||||
|
#include "sdk/os/zos.hpp"
|
||||
|
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, // 位置校正
|
||||
|
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, // 当前负载
|
||||
|
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; |
||||
|
|
||||
|
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(read8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;) |
||||
|
ZSTRUCT(read8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t data; uint8_t checksum;) |
||||
|
ZSTRUCT(write8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t regval; uint8_t checksum;) |
||||
|
ZSTRUCT(write8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) |
||||
|
ZSTRUCT(read16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;) |
||||
|
ZSTRUCT(read16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint16_t data; uint8_t checksum;) |
||||
|
ZSTRUCT(write16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint16_t regval; uint8_t checksum;) |
||||
|
ZSTRUCT(write16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) |
||||
|
|
||||
|
}; // namespace feite
|
||||
|
|
||||
|
using namespace feite; |
||||
|
class FeiTeServoMotor { |
||||
|
UART_HandleTypeDef* m_uart; |
||||
|
DMA_HandleTypeDef* m_hdma_rx; |
||||
|
DMA_HandleTypeDef* m_hdma_tx; |
||||
|
|
||||
|
public: |
||||
|
void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); |
||||
|
bool ping(uint8_t id); |
||||
|
|
||||
|
bool setmode(uint8_t id, run_mode_e runmode); |
||||
|
run_mode_e getmode(uint8_t id); |
||||
|
|
||||
|
bool setcurpos(int id, int16_t pos); |
||||
|
bool readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion); |
||||
|
bool readposcalibration(uint8_t id, int16_t& poscalibration); |
||||
|
|
||||
|
public: |
||||
|
bool write8(uint8_t id, feite::reg_add_e add, uint8_t regval); |
||||
|
bool read8(uint8_t id, feite::reg_add_e add, uint8_t& regval); |
||||
|
|
||||
|
bool write16(uint8_t id, feite::reg_add_e add, uint16_t regval); |
||||
|
bool read16(uint8_t id, feite::reg_add_e add, uint16_t& regval); |
||||
|
|
||||
|
bool read_s16(uint8_t id, feite::reg_add_e add, int16_t& regval); |
||||
|
bool write_s16(uint8_t id, feite::reg_add_e add, int16_t regval); |
||||
|
|
||||
|
bool async_write8(uint8_t id, feite::reg_add_e add, uint8_t regval); |
||||
|
bool async_write16(uint8_t id, feite::reg_add_e add, uint16_t regval); |
||||
|
|
||||
|
private: |
||||
|
bool tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems); |
||||
|
uint8_t checksum(uint8_t* data, uint8_t len); |
||||
|
uint8_t checksum_packet(uint8_t* data, uint8_t len); |
||||
|
}; |
||||
|
} // namespace iflytop
|
||||
@ -0,0 +1,48 @@ |
|||||
|
#include "mini_servo_motor_ctrl_module.hpp"
|
||||
|
|
||||
|
#include "sdk\components\errorcode\errorcode.hpp"
|
||||
|
using namespace iflytop; |
||||
|
using namespace std; |
||||
|
|
||||
|
void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write8(m_id, feite::kRegServoRunMode, feite::kServoMode); } |
||||
|
|
||||
|
int32_t MiniRobotCtrlModule::enable(u8 enable) { |
||||
|
bool suc = m_bus->write8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable); |
||||
|
if (!suc) return err::kce_subdevice_overtime; |
||||
|
return err::ksucc; |
||||
|
} |
||||
|
int32_t MiniRobotCtrlModule::stop(u8 stop_type) { |
||||
|
/**
|
||||
|
* @brief 切换下当前运行模式再切换回来，可以停止舵机运行 |
||||
|
*/ |
||||
|
if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; |
||||
|
feite::run_mode_e runmode = m_bus->getmode(m_id); |
||||
|
if (runmode != feite::kServoMode) { |
||||
|
m_bus->setmode(m_id, feite::kServoMode); |
||||
|
} else { |
||||
|
m_bus->setmode(m_id, feite::kMotorMode); |
||||
|
} |
||||
|
m_bus->setmode(m_id, runmode); |
||||
|
return err::ksucc; |
||||
|
} |
||||
|
int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) { |
||||
|
if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; |
||||
|
return 0; |
||||
|
} |
||||
|
int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::move_by(s32 pos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, function<void(move_by_nolimit_cb_status_t& status)> 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::read_debug_info(debug_info_t& debug_info) { return 0; } |
||||
|
|
||||
|
int32_t MiniRobotCtrlModule::set_run_param(run_param_t& param) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t& param) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& param) { return 0; } |
||||
|
|
||||
|
int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; } |
||||
|
int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; } |
||||
@ -0,0 +1,40 @@ |
|||||
|
#pragma once
|
||||
|
//
|
||||
|
#include "sdk/os/zos.hpp"
|
||||
|
//
|
||||
|
#include "feite_servo_motor.hpp"
|
||||
|
#include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp"
|
||||
|
|
||||
|
namespace iflytop { |
||||
|
class MiniRobotCtrlModule : public I_MiniServoModule { |
||||
|
FeiTeServoMotor* m_bus; |
||||
|
uint8_t m_id; |
||||
|
|
||||
|
s32 m_pos_shift; |
||||
|
|
||||
|
public: |
||||
|
void initialize(FeiTeServoMotor* bus, uint8_t id); |
||||
|
|
||||
|
virtual int32_t enable(u8 enable) override; |
||||
|
virtual int32_t stop(u8 stop_type) override; |
||||
|
virtual int32_t position_calibrate(s32 nowpos) override; |
||||
|
|
||||
|
virtual int32_t rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) override; |
||||
|
virtual int32_t move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) override; |
||||
|
virtual int32_t move_by(s32 pos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) override; |
||||
|
virtual int32_t run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) override; |
||||
|
virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, function<void(move_by_nolimit_cb_status_t& status)> status_cb) override; |
||||
|
|
||||
|
virtual int32_t read_version(version_t& version) override; |
||||
|
virtual int32_t read_status(status_t& status) override; |
||||
|
virtual int32_t read_debug_info(debug_info_t& debug_info) override; |
||||
|
|
||||
|
virtual int32_t set_run_param(run_param_t& param) override; |
||||
|
virtual int32_t set_basic_param(basic_param_t& param) override; |
||||
|
virtual int32_t set_warning_limit_param(warning_limit_param_t& param) override; |
||||
|
|
||||
|
virtual int32_t get_run_param(run_param_t& param) override; |
||||
|
virtual int32_t get_basic_param(basic_param_t& param) override; |
||||
|
virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override; |
||||
|
}; |
||||
|
} // namespace iflytop
|
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue