p_Intelligent_winding_robot_v2
/
Intelligent_winding_robot_main_board
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

去掉部分警告

master
zhaohe 2 years ago
parent
96f6146396
commit
ec71bb38f5
4 changed files with 4 additions and 584 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      .settings/language.settings.xml
  2. 2
      sdk
  3. 372
      usrc/feite_servo_motor.cpp
  4. 210
      usrc/feite_servo_motor.hpp

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-622244553529563973" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1057391452571803630" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-622244553529563973" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1057391452571803630" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>

2
sdk

@ -1 +1 @@
Subproject commit 2b35686d8d692c80c80bdbb6b7e4afa38d94f860
Subproject commit d69f8551a6307b256cb2917312e57e2949953db1

372
usrc/feite_servo_motor.cpp
View File

@ -1,372 +0,0 @@
#include "feite_servo_motor.hpp"
#include <stdint.h>
#include <string.h>
// #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, &regval, 1); }
bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, &regval, 1); }
bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)&regval, 2); }
bool FeiTeServoMotor::read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { return read_reg(id, add, (uint8_t*)&regval, 2); }
bool FeiTeServoMotor::async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, true, add, &regval, 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*)&regval, 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);
}

210
usrc/feite_servo_motor.hpp
View File

@ -1,210 +1,2 @@
#pragma once #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
#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp"
Write Preview
Loading…
Cancel
Save