project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update feite servo motor

master
zhaohe 2 years ago
parent
31a974763f
commit
463b0bf765
3 changed files with 107 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 124
      components/mini_servo_motor/feite_servo_motor.cpp
  2. 31
      components/mini_servo_motor/feite_servo_motor.hpp
  3. 2
      components/zprotocols/zcancmder

124
components/mini_servo_motor/feite_servo_motor.cpp
View File

@ -3,7 +3,7 @@ using namespace iflytop;
using namespace std;
using namespace feite;
#define TAG "FeiTeServoMotor"
#define OVERTIME 5
#define OVERTIME 30
#define DO(func) \
if (!(func)) { \
ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \
@ -67,13 +67,12 @@ bool FeiTeServoMotor::write16(uint8_t id, feite::reg_add_e add, uint16_t regval)
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) {
bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, 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;
uint8_t sign = (val >> signbitoff) & 0x01;
uint16_t realval = val & (~(1 << signbitoff));
if (sign == 0) {
regval = realval;
} else {
@ -82,13 +81,13 @@ bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, int16_t& regval
return true;
}
bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, int16_t regval) {
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 |= 0x8000;
val |= (1 << signbitoff);
}
return write16(id, add, val);
}
@ -136,7 +135,7 @@ bool FeiTeServoMotor::async_write16(uint8_t id, feite::reg_add_e add, uint16_t r
}
static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) {
int16_t calibrate = aftercalibratepos - nowpos;
int16_t calibrate = nowpos - aftercalibratepos;
while (true) {
if (calibrate > 2047) {
calibrate -= 4094;
@ -149,41 +148,78 @@ static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) {
return calibrate;
}
bool FeiTeServoMotor::setcurpos(int id, int16_t pos) {
/**
* @brief
* 1. 记录当前模式
* 2. 切换到位置模式
* 3. 读取当前位置
* 4. 读取当前位置校准数值
* 5. 计算新的校准值
* 6. 切换到速度模式（速度模式下不会因为位置的变化而导致舵机运动）
* 解锁写入
* 6. 写入新的校准值
* 锁定写入
* 7. 切换回原来的模式
*/
bool FeiTeServoMotor::write_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { //
return false;
}
bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { return false; }
bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write8(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 = read8(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 = read8(id, kRegServoRunMode, data);
runmode = (run_mode_e)data;
return suc;
}
bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write8(id, kRegServoTorqueSwitch, on ? 1 : 0); }
bool FeiTeServoMotor::getTorqueSwitch(uint8_t id, bool& on) {
uint8_t data = 0;
bool suc = read8(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::reCalibration(int id, int16_t pos) {
if (pos < 0 || pos > 4095) {
ZLOGE(TAG, "setcurpos pos:%d out of range", pos);
ZLOGE(TAG, "reCalibration pos:%d out of range", pos);
return false;
}
run_mode_e nowmode = getmode(id);
/**
* @brief 关闭扭矩快关，防止舵机运动
*/
setTorqueSwitch(id, false);
/**
* @brief 设置当前模式为位置模式
*/
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 curpos;
DO(getNowPos(id, curpos));
int16_t curcalibrate;
DO(getServoCalibration(id, curcalibrate));
int16_t realpos = curpos + curcalibrate;
int16_t newcalibrate = getcalibrate(realpos, pos);
DO(setmode(id, kMotorMode));
ZLOGI(TAG, "reCalibration id:%d curpos:%d curcalibrate:%d realpos:%d newcalibrate:%d", id, curpos, curcalibrate, realpos, newcalibrate);
/**
* @brief 写入新的校准值
*/
DO(write8(id, kRegServoLockFlag, 0));
DO(write_s16(id, kRegServoCalibration, newcalibrate));
DO(write_s16(id, kRegServoCalibration, 11, newcalibrate));
DO(write8(id, kRegServoLockFlag, 1));
DO(setmode(id, nowmode));
/**
* @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;
}
@ -193,21 +229,28 @@ bool FeiTeServoMotor::setcurpos(int id, int16_t pos) {
return false; \
}
static void dumphex(char* tag, uint8_t* data, uint8_t len) {
printf("%s:", tag);
for (int i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
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_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);
HAL_UART_Transmit(m_uart, tx, txdatalen, 1000);
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);
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) {
@ -237,9 +280,6 @@ bool FeiTeServoMotor::readversion(uint8_t id, uint8_t& mainversion, uint8_t& sub
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) {

31
components/mini_servo_motor/feite_servo_motor.hpp
View File

@ -49,7 +49,7 @@ typedef enum {
kRegServoProtectCurrent = 28, // 保护电流
kRegServoAngleResolution = 30, // 角度分辨
kRegServoCalibration = 31, // 位置校正
kRegServoCalibration = 31, // 位置校正 BIT11为方向位，表示正负方向，BIT0~10位表示范围0-2047步
kRegServoRunMode = 33, // 运行模式
kRegServoProtectTorque = 34, // 保护扭矩
kRegServoProtectTime = 35, // 保护时间
@ -82,7 +82,7 @@ typedef enum {
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(read8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;)
@ -94,6 +94,11 @@ ZSTRUCT(read16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_
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;)
ZSTRUCT(receipt_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd;)
ZSTRUCT(cmd_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd;)
#pragma pack()
}; // namespace feite
using namespace feite;
@ -105,13 +110,21 @@ class FeiTeServoMotor {
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);
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);
// 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);
public:
bool write8(uint8_t id, feite::reg_add_e add, uint8_t regval);
@ -120,12 +133,16 @@ class FeiTeServoMotor {
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 read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval);
bool write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, 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);
public:
bool write_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len);
bool read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len);
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);

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit b85eb03a3b5daf5b7f1d3e1e60a51a601824b08b
Subproject commit 8a33d6eeef9b51f409f6364235c7dbc26556fab5
Write Preview
Loading…
Cancel
Save