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

update

master
zhaohe 1 year ago
parent
b1c3dd52b9
commit
599ae6d11a
2 changed files with 179 additions and 93 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. 260
      components/mini_servo_motor/feite_servo_motor.cpp
  2. 12
      components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

260
components/mini_servo_motor/feite_servo_motor.cpp
View File

@ -7,15 +7,17 @@ using namespace iflytop;
using namespace std; using namespace std;
using namespace feite; using namespace feite;
#define TAG "FeiTeServoMotor" #define TAG "FeiTeServoMotor"
#define OVERTIME 30
#define OVERTIME 50
#define DO(func) \ #define DO(func) \
if (!(func)) { \ if (!(func)) { \
ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \ ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \
return false; \ return false; \
} }
#define DEBUG_MODE 1
static void dumphex(const char* tag, uint8_t* data, uint8_t len) { static void dumphex(const char* tag, uint8_t* data, uint8_t len) {
#if 0
#if DEBUG_MODE
printf("%s:", tag); printf("%s:", tag);
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
printf("%02x ", data[i]); printf("%02x ", data[i]);
@ -30,71 +32,74 @@ typedef struct {
uint8_t signoff; uint8_t signoff;
} feite_reg_info_t; } feite_reg_info_t;
feite_reg_info_t reginfo[255];
static feite_reg_info_t g_reginfo[255];
#define SIGNED true
#define UNSIGN false
#define REG_INFO_INIT(_regindex, _regwidth, _sign, _signoff) \ #define REG_INFO_INIT(_regindex, _regwidth, _sign, _signoff) \
{ \ { \
reginfo[_regindex].regwidth = _regwidth; \
reginfo[_regindex].sign = _sign; \
reginfo[_regindex].signoff = _signoff; \
g_reginfo[_regindex].regwidth = _regwidth; \
g_reginfo[_regindex].sign = _sign; \
g_reginfo[_regindex].signoff = _signoff; \
} }
static void reginfo_init() { static void reginfo_init() {
/*00*/ REG_INFO_INIT(kRegFirmwareMainVersion, 1, false, 0); // 固件主版本号
/*01*/ REG_INFO_INIT(kRegFirmwareSubVersion, 1, false, 0); // 固件次版本号
/*03*/ REG_INFO_INIT(kRegServoMainVersion, 1, false, 0); // 舵机主版本号
/*04*/ REG_INFO_INIT(kRegServoSubVersion, 1, false, 0); // 舵机次版本号
/*05*/ REG_INFO_INIT(kRegServoId, 1, false, 0); // ID
/*06*/ REG_INFO_INIT(kRegServoBaudRate, 1, false, 0); // 波特率
/*07*/ REG_INFO_INIT(kRegServoDelay, 1, false, 0); // 返回延时
/*08*/ REG_INFO_INIT(kRegServoAckLevel, 1, false, 0); // 应答状态级别
/*09*/ REG_INFO_INIT(kRegServoMinAngle, 2, false, 0); // 最小角度限制
/*11*/ REG_INFO_INIT(kRegServoMaxAngle, 2, false, 0); // 最大角度限制
/*13*/ REG_INFO_INIT(kRegServoMaxTemp, 1, false, 0); // 最高温度上限
/*14*/ REG_INFO_INIT(kRegServoMaxVoltage, 1, false, 0); // 最高输入电压
/*15*/ REG_INFO_INIT(kRegServoMinVoltage, 1, false, 0); // 最低输入电压
/*16*/ REG_INFO_INIT(kRegServoMaxTorque, 2, false, 0); // 最大扭矩
/*18*/ REG_INFO_INIT(kRegServoPhase, 1, false, 0); // 相位
/*19*/ REG_INFO_INIT(kRegServoUnloadCondition, 1, false, 0); // 卸载条件
/*20*/ REG_INFO_INIT(kRegServoLedAlarmCondition, 1, false, 0); // LED 报警条件
/*21*/ REG_INFO_INIT(kRegServoP, 1, false, 0); // P 比例系
/*22*/ REG_INFO_INIT(kRegServoD, 1, false, 0); // D 微分系
/*23*/ REG_INFO_INIT(kRegServoI, 1, false, 0); // I
/*24*/ REG_INFO_INIT(kRegServoMinStart, 2, false, 0); // 最小启动
/*26*/ REG_INFO_INIT(kRegServoCwDeadZone, 1, false, 0); // 顺时针不灵敏区
/*27*/ REG_INFO_INIT(kRegServoCcwDeadZone, 1, false, 0); // 逆时针不灵敏
/*28*/ REG_INFO_INIT(kRegServoProtectCurrent, 2, false, 0); // 保护电流
/*30*/ REG_INFO_INIT(kRegServoAngleResolution, 1, false, 0); // 角度分辨
/*31*/ REG_INFO_INIT(kRegServoCalibration, 2, true, 11); // 位置校正 BIT11为方向位，表示正负方向，BIT0~10位表示范围0-2047步
/*33*/ REG_INFO_INIT(kRegServoRunMode, 1, false, 0); // 运行模式
/*34*/ REG_INFO_INIT(kRegServoProtectTorque, 1, false, 0); // 保护扭矩
/*35*/ REG_INFO_INIT(kRegServoProtectTime, 1, false, 0); // 保护时间
/*36*/ REG_INFO_INIT(kRegServoOverloadTorque, 1, false, 0); // 过载扭矩
/*37*/ REG_INFO_INIT(kRegServoSpeedP, 1, false, 0); // 速度闭环P比例参数
/*38*/ REG_INFO_INIT(kRegServoOverloadTime, 1, false, 0); // 过流保护时间
/*39*/ REG_INFO_INIT(kRegServoSpeedI, 1, false, 0); // 速度闭环I积分参数
/*40*/ REG_INFO_INIT(kRegServoTorqueSwitch, 1, false, 0); // 扭矩开关
/*41*/ REG_INFO_INIT(kRegServoAcc, 1, false, 0); // 加速度
/*42*/ REG_INFO_INIT(kRegServoTargetPos, 2, true, 15); // 目标位置
/*44*/ REG_INFO_INIT(kRegServoRunTime, 2, true, 15); // 运行时间
/*46*/ REG_INFO_INIT(kRegServoRunSpeed, 2, true, 15); // 运行速度
/*48*/ REG_INFO_INIT(kRegServoTorqueLimit, 2, false, 0); // 转矩限制
/*55*/ REG_INFO_INIT(kRegServoLockFlag, 1, false, 0); // 锁标志
/*56*/ REG_INFO_INIT(kRegServoCurrentPos, 2, false, 0); // 当前位置
/*58*/ REG_INFO_INIT(kRegServoCurrentSpeed, 2, true, 15); // 当前速度
/*60*/ REG_INFO_INIT(kRegServoCurrentLoad, 2, false, 0); // 当前负载 bit10为方向位
/*62*/ REG_INFO_INIT(kRegServoCurrentVoltage, 1, false, 0); // 当前电压
/*63*/ REG_INFO_INIT(kRegServoCurrentTemp, 1, false, 0); // 当前温度
/*64*/ REG_INFO_INIT(kRegServoAsyncWriteFlag, 1, false, 0); // 异步写标志
/*65*/ REG_INFO_INIT(kRegServoStatus, 1, false, 0); // 舵机状态
/*66*/ REG_INFO_INIT(kRegServoMoveFlag, 1, false, 0); // 移动标志
/*69*/ REG_INFO_INIT(kRegServoCurrentCurrent, 2, false, 0); // 当前电流
/*80*/ REG_INFO_INIT(kRegServoCheckSpeed, 1, false, 0); // 80 移动检查速度
/*81*/ REG_INFO_INIT(kRegServoDTime, 1, false, 0); // 81 D控制时间
/*82*/ REG_INFO_INIT(kRegServoSpeedUnit, 1, false, 0); // 82 速度单位系数
/*83*/ REG_INFO_INIT(kRegServoMinSpeedLimit, 1, false, 0); // 83 最小速度限制
/*84*/ REG_INFO_INIT(kRegServoMaxSpeedLimit, 1, false, 0); // 84 最大速度限制
/*85*/ REG_INFO_INIT(kRegServoAccLimit, 1, false, 0); // 85 加速度限制
/*86*/ REG_INFO_INIT(kRegServoAccMultiple, 1, false, 0); // 86 加速度倍数
/*00*/ REG_INFO_INIT(kRegFirmwareMainVersion /* */, 1, UNSIGN, 0); // 固件主版本号
/*01*/ REG_INFO_INIT(kRegFirmwareSubVersion /* */, 1, UNSIGN, 0); // 固件次版本号
/*03*/ REG_INFO_INIT(kRegServoMainVersion /* */, 1, UNSIGN, 0); // 舵机主版本号
/*04*/ REG_INFO_INIT(kRegServoSubVersion /* */, 1, UNSIGN, 0); // 舵机次版本号
/*05*/ REG_INFO_INIT(kRegServoId /* */, 1, UNSIGN, 0); // ID
/*06*/ REG_INFO_INIT(kRegServoBaudRate /* */, 1, UNSIGN, 0); // 波特率
/*07*/ REG_INFO_INIT(kRegServoDelay /* */, 1, UNSIGN, 0); // 返回延时
/*08*/ REG_INFO_INIT(kRegServoAckLevel /* */, 1, UNSIGN, 0); // 应答状态级别
/*09*/ REG_INFO_INIT(kRegServoMinAngle /* */, 2, UNSIGN, 0); // 最小角度限制
/*11*/ REG_INFO_INIT(kRegServoMaxAngle /* */, 2, UNSIGN, 0); // 最大角度限制
/*13*/ REG_INFO_INIT(kRegServoMaxTemp /* */, 1, UNSIGN, 0); // 最高温度上限
/*14*/ REG_INFO_INIT(kRegServoMaxVoltage /* */, 1, UNSIGN, 0); // 最高输入电压
/*15*/ REG_INFO_INIT(kRegServoMinVoltage /* */, 1, UNSIGN, 0); // 最低输入电压
/*16*/ REG_INFO_INIT(kRegServoMaxTorque /* */, 2, UNSIGN, 0); // 最大扭矩
/*18*/ REG_INFO_INIT(kRegServoPhase /* */, 1, UNSIGN, 0); // 相位
/*19*/ REG_INFO_INIT(kRegServoUnloadCondition /* */, 1, UNSIGN, 0); // 卸载条件
/*20*/ REG_INFO_INIT(kRegServoLedAlarmCondition /* */, 1, UNSIGN, 0); // LED 报警条件
/*21*/ REG_INFO_INIT(kRegServoP /* */, 1, UNSIGN, 0); // P 比例系
/*22*/ REG_INFO_INIT(kRegServoD /* */, 1, UNSIGN, 0); // D 微分系
/*23*/ REG_INFO_INIT(kRegServoI /* */, 1, UNSIGN, 0); // I
/*24*/ REG_INFO_INIT(kRegServoMinStart /* */, 2, UNSIGN, 0); // 最小启动
/*26*/ REG_INFO_INIT(kRegServoCwDeadZone /* */, 1, UNSIGN, 0); // 顺时针不灵敏区
/*27*/ REG_INFO_INIT(kRegServoCcwDeadZone /* */, 1, UNSIGN, 0); // 逆时针不灵敏
/*28*/ REG_INFO_INIT(kRegServoProtectCurrent /* */, 2, UNSIGN, 0); // 保护电流
/*30*/ REG_INFO_INIT(kRegServoAngleResolution /* */, 1, UNSIGN, 0); // 角度分辨
/*31*/ REG_INFO_INIT(kRegServoCalibration /* */, 2, SIGNED, 11); // 位置校正 BIT11为方向位，表示正负方向，BIT0~10位表示范围0-2047步
/*33*/ REG_INFO_INIT(kRegServoRunMode /* */, 1, UNSIGN, 0); // 运行模式
/*34*/ REG_INFO_INIT(kRegServoProtectTorque /* */, 1, UNSIGN, 0); // 保护扭矩
/*35*/ REG_INFO_INIT(kRegServoProtectTime /* */, 1, UNSIGN, 0); // 保护时间
/*36*/ REG_INFO_INIT(kRegServoOverloadTorque /* */, 1, UNSIGN, 0); // 过载扭矩
/*37*/ REG_INFO_INIT(kRegServoSpeedP /* */, 1, UNSIGN, 0); // 速度闭环P比例参数
/*38*/ REG_INFO_INIT(kRegServoOverloadTime /* */, 1, UNSIGN, 0); // 过流保护时间
/*39*/ REG_INFO_INIT(kRegServoSpeedI /* */, 1, UNSIGN, 0); // 速度闭环I积分参数
/*40*/ REG_INFO_INIT(kRegServoTorqueSwitch /* */, 1, UNSIGN, 0); // 扭矩开关
/*41*/ REG_INFO_INIT(kRegServoAcc /* */, 1, UNSIGN, 0); // 加速度
/*42*/ REG_INFO_INIT(kRegServoTargetPos /* */, 2, UNSIGN, 0); // 目标位置
/*44*/ REG_INFO_INIT(kRegServoRunTime /* */, 2, SIGNED, 10); // 运行时间
/*46*/ REG_INFO_INIT(kRegServoRunSpeed /* */, 2, SIGNED, 15); // 运行速度
/*48*/ REG_INFO_INIT(kRegServoTorqueLimit /* */, 2, UNSIGN, 0); // 转矩限制
/*55*/ REG_INFO_INIT(kRegServoLockFlag /* */, 1, UNSIGN, 0); // 锁标志
/*56*/ REG_INFO_INIT(kRegServoCurrentPos /* */, 2, UNSIGN, 0); // 当前位置
/*58*/ REG_INFO_INIT(kRegServoCurrentSpeed /* */, 2, SIGNED, 15); // 当前速度
/*60*/ REG_INFO_INIT(kRegServoCurrentLoad /* */, 2, UNSIGN, 0); // 当前负载 bit10为方向位
/*62*/ REG_INFO_INIT(kRegServoCurrentVoltage /* */, 1, UNSIGN, 0); // 当前电压
/*63*/ REG_INFO_INIT(kRegServoCurrentTemp /* */, 1, UNSIGN, 0); // 当前温度
/*64*/ REG_INFO_INIT(kRegServoAsyncWriteFlag /* */, 1, UNSIGN, 0); // 异步写标志
/*65*/ REG_INFO_INIT(kRegServoStatus /* */, 1, UNSIGN, 0); // 舵机状态
/*66*/ REG_INFO_INIT(kRegServoMoveFlag /* */, 1, UNSIGN, 0); // 移动标志
/*69*/ REG_INFO_INIT(kRegServoCurrentCurrent /* */, 2, UNSIGN, 0); // 当前电流
/*80*/ REG_INFO_INIT(kRegServoCheckSpeed /* */, 1, UNSIGN, 0); // 80 移动检查速度
/*81*/ REG_INFO_INIT(kRegServoDTime /* */, 1, UNSIGN, 0); // 81 D控制时间
/*82*/ REG_INFO_INIT(kRegServoSpeedUnit /* */, 1, UNSIGN, 0); // 82 速度单位系数
/*83*/ REG_INFO_INIT(kRegServoMinSpeedLimit /* */, 1, UNSIGN, 0); // 83 最小速度限制
/*84*/ REG_INFO_INIT(kRegServoMaxSpeedLimit /* */, 1, UNSIGN, 0); // 84 最大速度限制
/*85*/ REG_INFO_INIT(kRegServoAccLimit /* */, 1, UNSIGN, 0); // 85 加速度限制
/*86*/ REG_INFO_INIT(kRegServoAccMultiple /* */, 1, UNSIGN, 0); // 86 加速度倍数
} }
void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type) { void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type) {
@ -124,6 +129,7 @@ void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type
info.regDefaultVal_servoMaxAngle = 1003; info.regDefaultVal_servoMaxAngle = 1003;
info.angleConverCoefficient = 300 / 1024.0; info.angleConverCoefficient = 300 / 1024.0;
} }
regServoInfo(&info);
} }
void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) { void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) {
@ -134,6 +140,7 @@ void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hd
ZASSERT(hdma_tx->Init.Direction == DMA_MEMORY_TO_PERIPH); ZASSERT(hdma_tx->Init.Direction == DMA_MEMORY_TO_PERIPH);
m_mutex.init(); m_mutex.init();
reginfo_init();
} }
bool FeiTeServoMotor::ping(uint8_t id) { bool FeiTeServoMotor::ping(uint8_t id) {
zlock_guard l(m_mutex); zlock_guard l(m_mutex);
@ -160,10 +167,13 @@ bool FeiTeServoMotor::stop(uint8_t id) {
DO(write_reg(id, kRegServoTorqueSwitch, 0)); DO(write_reg(id, kRegServoTorqueSwitch, 0));
DO(write_reg(id, kRegServoRunTime, 0)); DO(write_reg(id, kRegServoRunTime, 0));
DO(write_reg(id, kRegServoTorqueSwitch, 1)); DO(write_reg(id, kRegServoTorqueSwitch, 1));
return true;
} else { } else {
DO(write_reg(id, kRegServoTorqueSwitch, 0)); DO(write_reg(id, kRegServoTorqueSwitch, 0));
DO(write_reg(id, kRegServoRunMode, 0)); DO(write_reg(id, kRegServoRunMode, 0));
DO(write_reg(id, kRegServoTorqueSwitch, 1)); DO(write_reg(id, kRegServoTorqueSwitch, 1));
return true;
} }
} }
@ -177,11 +187,11 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
} }
if (info->isSCS) { if (info->isSCS) {
if (limitTorque <= 0) limitTorque = 1000;
if (limitTorque <= 0) limitTorque = 0;
if (limitTorque > 1000) limitTorque = 1000; if (limitTorque > 1000) limitTorque = 1000;
// pos 范围为固定的 0->3600 // pos 范围为固定的 0->3600
int32_t motor_pos = pos3600 / 10.0 * info->angleConverCoefficient;
int32_t motor_pos = pos3600 / 10.0 / info->angleConverCoefficient;
if (motor_pos < info->regDefaultVal_servoMinAngle) motor_pos = info->regDefaultVal_servoMinAngle; if (motor_pos < info->regDefaultVal_servoMinAngle) motor_pos = info->regDefaultVal_servoMinAngle;
if (motor_pos > info->regDefaultVal_servoMaxAngle) motor_pos = info->regDefaultVal_servoMaxAngle; if (motor_pos > info->regDefaultVal_servoMaxAngle) motor_pos = info->regDefaultVal_servoMaxAngle;
@ -195,21 +205,26 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
DO(write_reg(id, kRegServoRunSpeed, speed)); DO(write_reg(id, kRegServoRunSpeed, speed));
DO(write_reg(id, kRegServoTargetPos, motor_pos)); DO(write_reg(id, kRegServoTargetPos, motor_pos));
return true;
} else { } else {
if (limitTorque <= 0) limitTorque = 1000;
if (limitTorque <= 0) limitTorque = 0;
if (limitTorque > 1000) limitTorque = 1000; if (limitTorque > 1000) limitTorque = 1000;
// pos 范围为固定的 0->3600 // pos 范围为固定的 0->3600
int32_t motor_pos = pos3600 / 10.0 * info->angleConverCoefficient;
int32_t motor_pos = (pos3600 / 10.0) / info->angleConverCoefficient;
// int32_t motor_pos = pos3600;
if (motor_pos < info->regDefaultVal_servoMinAngle) motor_pos = info->regDefaultVal_servoMinAngle; if (motor_pos < info->regDefaultVal_servoMinAngle) motor_pos = info->regDefaultVal_servoMinAngle;
if (motor_pos > info->regDefaultVal_servoMaxAngle) motor_pos = info->regDefaultVal_servoMaxAngle; if (motor_pos > info->regDefaultVal_servoMaxAngle) motor_pos = info->regDefaultVal_servoMaxAngle;
stop(id);
// stop(id);
int32_t val = 0;
DO(write_reg(id, kRegServoTorqueSwitch, 1));
DO(write_reg(id, kRegServoRunMode, (uint8_t)0)); DO(write_reg(id, kRegServoRunMode, (uint8_t)0));
DO(write_reg(id, kRegServoTorqueLimit, limitTorque)); DO(write_reg(id, kRegServoTorqueLimit, limitTorque));
DO(write_reg(id, kRegServoRunSpeed, speed)); DO(write_reg(id, kRegServoRunSpeed, speed));
DO(write_reg(id, kRegServoTargetPos, motor_pos)); DO(write_reg(id, kRegServoTargetPos, motor_pos));
return true;
} }
} }
@ -240,12 +255,15 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed)
ZLOGE(TAG, "runInMode0 id:%d not find", id); ZLOGE(TAG, "runInMode0 id:%d not find", id);
return false; return false;
} }
if (!info->support_mode1) {
ZLOGE(TAG, "runInMode1 id:%d not support", id);
return false;
}
if (info->isSCS) { if (info->isSCS) {
ZLOGE(TAG, "runInMode1 id:%d not support", id); ZLOGE(TAG, "runInMode1 id:%d not support", id);
return false; return false;
} else { } else {
if (limitTorque <= 0) limitTorque = 1000;
if (limitTorque <= 0) limitTorque = 0;
if (limitTorque > 1000) limitTorque = 1000; if (limitTorque > 1000) limitTorque = 1000;
stop(id); stop(id);
@ -264,21 +282,56 @@ bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) {
ZLOGE(TAG, "runInMode0 id:%d not find", id); ZLOGE(TAG, "runInMode0 id:%d not find", id);
return false; return false;
} }
if (!info->support_mode2) {
ZLOGE(TAG, "runInMode2 id:%d not support", id);
return false;
}
if (info->isSCS) { if (info->isSCS) {
stop(id); stop(id);
DO(write_reg(id, kRegServoMinAngle, 0)); DO(write_reg(id, kRegServoMinAngle, 0));
DO(write_reg(id, kRegServoMaxAngle, 0)); DO(write_reg(id, kRegServoMaxAngle, 0));
if (abs(torque) > 300) {
int direction = 1;
if (torque < 0) direction = -1;
DO(write_reg(id, kRegServoRunTime, direction * 100));
osDelay(100);
DO(write_reg(id, kRegServoRunTime, direction * 200));
osDelay(100);
}
DO(write_reg(id, kRegServoRunTime, torque)); DO(write_reg(id, kRegServoRunTime, torque));
return true; return true;
} else { } else {
if (torque <= 0) torque = 1000;
if (torque > 1000) torque = 1000;
stop(id);
#if 0
int32_t limittorque = abs(torque);
if (limittorque > 1000) limittorque = 1000;
// stop(id);
DO(write_reg(id, kRegServoRunMode, (uint8_t)2)); DO(write_reg(id, kRegServoRunMode, (uint8_t)2));
// DO(write_reg(id, kRegServoTorqueLimit, 1000));
osDelay(100);
if (abs(torque) > 300) {
int direction = 1;
if (torque < 0) direction = -1;
DO(write_reg(id, kRegServoRunTime, direction * 100));
osDelay(100);
DO(write_reg(id, kRegServoRunTime, direction * 200));
osDelay(100);
}
DO(write_reg(id, kRegServoRunTime, torque)); DO(write_reg(id, kRegServoRunTime, torque));
return true;
#endif
int32_t velocity = 10000;
if (torque < 0) {
velocity = -10000;
} else if (torque == 0) {
velocity = 0;
}
return runInMode1(id, abs(torque), velocity);
} }
return false; return false;
} }
@ -290,7 +343,9 @@ bool FeiTeServoMotor::getPos(uint8_t id, int32_t* pos) {
bool suc = read_reg(id, kRegServoCurrentPos, pos); bool suc = read_reg(id, kRegServoCurrentPos, pos);
if (!suc) return false; if (!suc) return false;
*pos = (*pos) * info->angleConverCoefficient * 10;
// ZLOGI(TAG, "getPos id:%d pos:%d", id, *pos);
*pos = (*pos) * info->angleConverCoefficient * 10 + 0.5;
return true; return true;
} }
bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) { bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) {
@ -300,7 +355,10 @@ bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) {
bool FeiTeServoMotor::write_reg(uint8_t id, int regadd, int32_t regval) { bool FeiTeServoMotor::write_reg(uint8_t id, int regadd, int32_t regval) {
feite_servo_info_t* servoInfo = getServoInfo(id); feite_servo_info_t* servoInfo = getServoInfo(id);
feite_reg_info_t* reginfo = &reginfo[regadd];
feite_reg_info_t* reginfo = &g_reginfo[regadd];
#if DEBUG_MODE
ZLOGI(TAG, "write_reg id:%d , regadd:%d , regval:%d", id, regadd, regval);
#endif
if (regadd > 255) { if (regadd > 255) {
ZLOGE(TAG, "write_reg id:%d , regadd:%d out of range", id, regadd); ZLOGE(TAG, "write_reg id:%d , regadd:%d out of range", id, regadd);
return false; return false;
@ -311,15 +369,27 @@ bool FeiTeServoMotor::write_reg(uint8_t id, int regadd, int32_t regval) {
return false; return false;
} }
int32_t absval = abs(regval);
uint8_t writebuf[2];
int32_t absval = abs(regval);
uint8_t writebuf[2] = {0};
uint16_t* writebuf_u16p = (uint16_t*)writebuf; uint16_t* writebuf_u16p = (uint16_t*)writebuf;
int16_t* writebuf_16p = (int16_t*)writebuf;
if (reginfo->regwidth == 1) { if (reginfo->regwidth == 1) {
writebuf[0] = regval; writebuf[0] = regval;
return write_regs(id, false, regadd, writebuf, 1); return write_regs(id, false, regadd, writebuf, 1);
} else if (reginfo->regwidth == 2) { } else if (reginfo->regwidth == 2) {
// if (regadd == kRegServoRunTime) {
// *writebuf_16p = regval;
// if (servoInfo->bigend) {
// uint8_t tmp = writebuf[0];
// writebuf[0] = writebuf[1];
// writebuf[1] = tmp;
// }
// return write_regs(id, false, regadd, writebuf, 2);
// } else {
*writebuf_u16p = absval; *writebuf_u16p = absval;
if (reginfo->sign && regval > 0) {
if (reginfo->sign && regval < 0) {
*writebuf_u16p |= (1 << reginfo->signoff); *writebuf_u16p |= (1 << reginfo->signoff);
} }
@ -328,7 +398,9 @@ bool FeiTeServoMotor::write_reg(uint8_t id, int regadd, int32_t regval) {
writebuf[0] = writebuf[1]; writebuf[0] = writebuf[1];
writebuf[1] = tmp; writebuf[1] = tmp;
} }
return write_regs(id, false, regadd, writebuf, 2); return write_regs(id, false, regadd, writebuf, 2);
// }
} else { } else {
ZLOGE(TAG, "write_reg id:%d , regadd:%d regwidth:%d not support", id, regadd, reginfo->regwidth); ZLOGE(TAG, "write_reg id:%d , regadd:%d regwidth:%d not support", id, regadd, reginfo->regwidth);
@ -337,7 +409,7 @@ bool FeiTeServoMotor::write_reg(uint8_t id, int regadd, int32_t regval) {
} }
bool FeiTeServoMotor::read_reg(uint8_t id, int regadd, int32_t* regval) { bool FeiTeServoMotor::read_reg(uint8_t id, int regadd, int32_t* regval) {
feite_servo_info_t* servoInfo = getServoInfo(id); feite_servo_info_t* servoInfo = getServoInfo(id);
feite_reg_info_t* reginfo = &reginfo[regadd];
feite_reg_info_t* reginfo = &g_reginfo[regadd];
if (regadd > 255) { if (regadd > 255) {
ZLOGE(TAG, "read_reg id:%d , regadd:%d out of range", id, regadd); ZLOGE(TAG, "read_reg id:%d , regadd:%d out of range", id, regadd);
return false; return false;
@ -366,9 +438,13 @@ bool FeiTeServoMotor::read_reg(uint8_t id, int regadd, int32_t* regval) {
readbuf[1] = tmp; readbuf[1] = tmp;
} }
uint8_t sign = (*readbuf_u16p >> reginfo->signoff) & 0x01;
if (sign) {
*regval = -(*readbuf_u16p & (~(1 << reginfo->signoff)));
if (reginfo->sign) {
uint8_t sign = (*readbuf_u16p >> reginfo->signoff) & 0x01;
if (sign) {
*regval = -(*readbuf_u16p & (~(1 << reginfo->signoff)));
} else {
*regval = *readbuf_u16p;
}
} else { } else {
*regval = *readbuf_u16p; *regval = *readbuf_u16p;
} }
@ -380,8 +456,10 @@ bool FeiTeServoMotor::read_reg(uint8_t id, int regadd, int32_t* regval) {
} }
bool FeiTeServoMotor::write_regs(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // bool FeiTeServoMotor::write_regs(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);
zlock_guard l(m_mutex); zlock_guard l(m_mutex);
#if DEBUG_MODE
ZLOGI(TAG, " write_regs id:%d add:%d len:%d %x %x", id, add, len, data[0], data[1]);
#endif
cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf;
receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf;
@ -401,11 +479,16 @@ bool FeiTeServoMotor::write_regs(uint8_t id, bool async, uint8_t add, uint8_t* d
uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen);
m_txbuf[txpacketlen - 1] = checksum; m_txbuf[txpacketlen - 1] = checksum;
if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
ZLOGE(TAG, "%d write_reg fail,overtime", id);
ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,overtime", id, add, len);
return false; return false;
} }
if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
ZLOGE(TAG, "%d write_reg fail,receipt header error", id);
ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,receipt header error", id, add, len);
return false;
}
if (receipt_header->status != 0 && add != kRegServoTorqueSwitch) {
ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,receipt status error %d", id, add, len, receipt_header->status);
return false; return false;
} }
return true; return true;
@ -433,9 +516,11 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t
m_txbuf[txpacketlen - 1] = checksum; m_txbuf[txpacketlen - 1] = checksum;
if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
ZLOGE(TAG, "read_reg id:%d add:%d len:%d fail,overtime", id, add, len);
return false; return false;
} }
if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
ZLOGE(TAG, "read_reg id:%d add:%d len:%d fail,receipt header error", id, add, len);
ZLOGE(TAG, "read_reg fail,receipt header error"); ZLOGE(TAG, "read_reg fail,receipt header error");
return false; return false;
} }
@ -445,9 +530,6 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t
bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) { bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) {
uint32_t enter_ticket = HAL_GetTick(); uint32_t enter_ticket = HAL_GetTick();
dumphex("tx:", tx, txdatalen); dumphex("tx:", tx, txdatalen);
// clear rx buffer
#if 1 #if 1
HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize); HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize);
HAL_UART_Transmit(m_uart, tx, txdatalen, 1000); HAL_UART_Transmit(m_uart, tx, txdatalen, 1000);
@ -498,4 +580,4 @@ feite_servo_info_t* FeiTeServoMotor::getServoInfo(int id) {
} }
} }
return nullptr; return nullptr;
}
}

12
components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
View File

@ -21,8 +21,8 @@ void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, u
m_cfg = *cfg; m_cfg = *cfg;
m_bus->write_reg(m_idinbus, feite::kRegServoRunMode, feite::kServoMode);
m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal); m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
mini_servo_enable(1);
} }
void MiniServoCtrlModule::create_default_config(config_t &cfg) { void MiniServoCtrlModule::create_default_config(config_t &cfg) {
cfg.limit_velocity = 10000; cfg.limit_velocity = 10000;
@ -38,7 +38,7 @@ int32_t MiniServoCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
switch (param_id) { switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB(); MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_mini_servo_pos, mini_servo_read_pos(&val), ACTION_NONE); PROCESS_REG(kreg_mini_servo_pos, mini_servo_read_pos(&val), ACTION_NONE);
PROCESS_REG(kreg_mini_servo_limit_velocity, REG_GET(m_cfg.limit_velocity), ACTION_NONE);
PROCESS_REG(kreg_mini_servo_limit_velocity, REG_GET(m_cfg.limit_velocity), REG_SET(m_cfg.limit_velocity));
PROCESS_REG(kreg_mini_servo_limit_torque, REG_GET(m_cfg.limit_torque), REG_SET(m_cfg.limit_torque)); PROCESS_REG(kreg_mini_servo_limit_torque, REG_GET(m_cfg.limit_torque), REG_SET(m_cfg.limit_torque));
PROCESS_REG(kreg_mini_servo_protective_torque, REG_GET(m_cfg.protective_torque), REG_SET(m_cfg.protective_torque)); PROCESS_REG(kreg_mini_servo_protective_torque, REG_GET(m_cfg.protective_torque), REG_SET(m_cfg.protective_torque));
default: default:
@ -60,7 +60,10 @@ int32_t MiniServoCtrlModule::mini_servo_read_pos(int32_t *pos) {
return 0; return 0;
} }
int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; } int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; }
int32_t MiniServoCtrlModule::mini_servo_stop(int32_t breakstop) { return inter_mini_servo_stop(breakstop); }
int32_t MiniServoCtrlModule::mini_servo_stop(int32_t breakstop) {
m_thread.stop();
return inter_mini_servo_stop(breakstop);
}
int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) { int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) {
ZLOGI(TAG, "%d mini_servo_rotate %d", m_module_id, speed); ZLOGI(TAG, "%d mini_servo_rotate %d", m_module_id, speed);
@ -123,8 +126,9 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
} }
after_motor_move(); after_motor_move();
ZLOGI(TAG, "move to pos %d", pos3600);
}, },
[this]() {});
[this]() { inter_mini_servo_stop(0); });
return 0; return 0;
} }

Write Preview
Loading…
Cancel
Save