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.
372 lines
13 KiB
372 lines
13 KiB
#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, ®val, 1); }
|
|
bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, ®val, 1); }
|
|
bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)®val, 2); }
|
|
bool FeiTeServoMotor::read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { return read_reg(id, add, (uint8_t*)®val, 2); }
|
|
bool FeiTeServoMotor::async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, true, add, ®val, 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*)®val, 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);
|
|
}
|