From 599ae6d11aaf71a05f8cc2436a230b2e880d43d0 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 4 Jun 2024 16:02:12 +0800 Subject: [PATCH] update --- components/mini_servo_motor/feite_servo_motor.cpp | 260 ++++++++++++++------- .../mini_servo_motor_ctrl_module.cpp | 12 +- 2 files changed, 179 insertions(+), 93 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index 9de008e..ffe2334 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -7,15 +7,17 @@ using namespace iflytop; using namespace std; using namespace feite; #define TAG "FeiTeServoMotor" -#define OVERTIME 30 +#define OVERTIME 50 #define DO(func) \ if (!(func)) { \ ZLOGE(TAG, "motor[%d] do %s fail", id, #func); \ return false; \ } +#define DEBUG_MODE 1 + static void dumphex(const char* tag, uint8_t* data, uint8_t len) { -#if 0 +#if DEBUG_MODE printf("%s:", tag); for (int i = 0; i < len; i++) { printf("%02x ", data[i]); @@ -30,71 +32,74 @@ typedef struct { uint8_t signoff; } 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) \ { \ - 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() { - /*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) { @@ -124,6 +129,7 @@ void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type info.regDefaultVal_servoMaxAngle = 1003; info.angleConverCoefficient = 300 / 1024.0; } + regServoInfo(&info); } 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); m_mutex.init(); + reginfo_init(); } bool FeiTeServoMotor::ping(uint8_t id) { 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, kRegServoRunTime, 0)); DO(write_reg(id, kRegServoTorqueSwitch, 1)); + return true; + } else { DO(write_reg(id, kRegServoTorqueSwitch, 0)); DO(write_reg(id, kRegServoRunMode, 0)); 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 (limitTorque <= 0) limitTorque = 1000; + if (limitTorque <= 0) limitTorque = 0; if (limitTorque > 1000) limitTorque = 1000; // 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_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, kRegServoTargetPos, motor_pos)); + return true; } else { - if (limitTorque <= 0) limitTorque = 1000; + if (limitTorque <= 0) limitTorque = 0; if (limitTorque > 1000) limitTorque = 1000; // 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_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, kRegServoTorqueLimit, limitTorque)); DO(write_reg(id, kRegServoRunSpeed, speed)); 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); return false; } - + if (!info->support_mode1) { + ZLOGE(TAG, "runInMode1 id:%d not support", id); + return false; + } if (info->isSCS) { ZLOGE(TAG, "runInMode1 id:%d not support", id); return false; } else { - if (limitTorque <= 0) limitTorque = 1000; + if (limitTorque <= 0) limitTorque = 0; if (limitTorque > 1000) limitTorque = 1000; stop(id); @@ -264,21 +282,56 @@ bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) { ZLOGE(TAG, "runInMode0 id:%d not find", id); return false; } + if (!info->support_mode2) { + ZLOGE(TAG, "runInMode2 id:%d not support", id); + return false; + } if (info->isSCS) { stop(id); DO(write_reg(id, kRegServoMinAngle, 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)); + return true; } 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, 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)); - 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; } @@ -290,7 +343,9 @@ bool FeiTeServoMotor::getPos(uint8_t id, int32_t* pos) { bool suc = read_reg(id, kRegServoCurrentPos, pos); 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; } 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) { feite_servo_info_t* servoInfo = getServoInfo(id); - feite_reg_info_t* reginfo = ®info[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) { ZLOGE(TAG, "write_reg id:%d , regadd:%d out of range", id, regadd); return false; @@ -311,15 +369,27 @@ bool FeiTeServoMotor::write_reg(uint8_t id, int regadd, int32_t regval) { 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; + int16_t* writebuf_16p = (int16_t*)writebuf; if (reginfo->regwidth == 1) { writebuf[0] = regval; return write_regs(id, false, regadd, writebuf, 1); } 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; - if (reginfo->sign && regval > 0) { + + if (reginfo->sign && regval < 0) { *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[1] = tmp; } + return write_regs(id, false, regadd, writebuf, 2); + // } } else { 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) { feite_servo_info_t* servoInfo = getServoInfo(id); - feite_reg_info_t* reginfo = ®info[regadd]; + feite_reg_info_t* reginfo = &g_reginfo[regadd]; if (regadd > 255) { ZLOGE(TAG, "read_reg id:%d , regadd:%d out of range", id, regadd); return false; @@ -366,9 +438,13 @@ bool FeiTeServoMotor::read_reg(uint8_t id, int regadd, int32_t* regval) { 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 { *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) { // - // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); 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; 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); m_txbuf[txpacketlen - 1] = checksum; 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; } 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 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; 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; } 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"); 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) { uint32_t enter_ticket = HAL_GetTick(); dumphex("tx:", tx, txdatalen); - - // clear rx buffer - #if 1 HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize); HAL_UART_Transmit(m_uart, tx, txdatalen, 1000); @@ -498,4 +580,4 @@ feite_servo_info_t* FeiTeServoMotor::getServoInfo(int id) { } } return nullptr; -} \ No newline at end of file +} diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index e8043b9..8739299 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -21,8 +21,8 @@ void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, u m_cfg = *cfg; - m_bus->write_reg(m_idinbus, feite::kRegServoRunMode, feite::kServoMode); m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal); + mini_servo_enable(1); } void MiniServoCtrlModule::create_default_config(config_t &cfg) { 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) { MODULE_COMMON_PROCESS_REG_CB(); 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_protective_torque, REG_GET(m_cfg.protective_torque), REG_SET(m_cfg.protective_torque)); default: @@ -60,7 +60,10 @@ int32_t MiniServoCtrlModule::mini_servo_read_pos(int32_t *pos) { 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) { 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(); + ZLOGI(TAG, "move to pos %d", pos3600); }, - [this]() {}); + [this]() { inter_mini_servo_stop(0); }); return 0; }