From d08cf7c7877ed4c9fcb9c84b496f2f0fb631d70f Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 21 Jul 2024 18:18:02 +0800 Subject: [PATCH] =?UTF-8?q?=E9=A3=9E=E7=89=B9=E8=88=B5=E6=9C=BA=E6=94=AF?= =?UTF-8?q?=E6=8C=81=E8=AE=BE=E7=BD=AE=E4=BB=BB=E6=84=8F=E4=BD=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- a8000_protocol | 2 +- .../mini_servo_motor/feite_servo_motor.cpp | 184 ++++++++++++++++++++- .../mini_servo_motor/feite_servo_motor.hpp | 15 +- .../mini_servo_motor_ctrl_module.cpp | 28 +++- .../mini_servo_motor_ctrl_module.hpp | 4 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 3 +- sdk/components/zcancmder/zcan_protocol_parser.cpp | 5 + sdk/components/zcancmder/zcan_protocol_parser.hpp | 3 +- .../subboard30_shake_module.cpp | 8 +- .../subboard90_optical_module/pri_board.h | 6 +- 10 files changed, 233 insertions(+), 25 deletions(-) diff --git a/a8000_protocol b/a8000_protocol index 36523be..ad55d44 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit 36523bea3565257b6033f75fd1adb343602532e9 +Subproject commit ad55d4474d5fe39585c788d2f5d73832cf782e94 diff --git a/sdk/components/mini_servo_motor/feite_servo_motor.cpp b/sdk/components/mini_servo_motor/feite_servo_motor.cpp index 4d2a16f..1ef62a3 100644 --- a/sdk/components/mini_servo_motor/feite_servo_motor.cpp +++ b/sdk/components/mini_servo_motor/feite_servo_motor.cpp @@ -102,10 +102,11 @@ static void reginfo_init() { /*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, bool shaft) { feite_servo_info_t info; info.id = id; info.model_type = model_type; + info.shaft = shaft; if (info.model_type == ksts5420m_c001) { info.support_mode0 = true; info.support_mode1 = true; @@ -116,8 +117,8 @@ void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type info.regDefaultVal_servoMinAngle = 1; info.regDefaultVal_servoMaxAngle = 4094; - - info.angleConverCoefficient = 360 / 4096.0; + info.range = 4096; + info.angleConverCoefficient = 360 / 4096.0; } else if (info.model_type == kscs009) { info.support_mode0 = true; info.support_mode1 = false; @@ -127,6 +128,7 @@ void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type info.isSCS = true; info.regDefaultVal_servoMinAngle = 20; info.regDefaultVal_servoMaxAngle = 1003; + info.range = 1024; info.angleConverCoefficient = 300 / 1024.0; } regServoInfo(&info); @@ -187,6 +189,12 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed, } if (info->isSCS) { + if (info->shaft) { + pos3600 = 3600 - pos3600; + } + if (pos3600 < 0) { + pos3600 = 0; + } if (limitTorque <= 0) limitTorque = 0; if (limitTorque > 1000) limitTorque = 1000; @@ -207,6 +215,12 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed, return true; } else { + if (info->shaft) { + pos3600 = 3600 - pos3600; + } + if (pos3600 < 0) { + pos3600 = 0; + } if (limitTorque <= 0) limitTorque = 0; if (limitTorque > 1000) limitTorque = 1000; @@ -233,6 +247,75 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed, } } +bool FeiTeServoMotor::setInMode0(uint8_t id) { + zlock_guard l(m_mutex); + feite_servo_info_t* info = getServoInfo(id); + + if (!info) { + ZLOGE(TAG, "runInMode0 id:%d not find", id); + return false; + } + + if (info->isSCS) { + // 由于SCS型号的舵机如果当前工作在模式2下， + // 则 kRegServoRunTime 控制运行方向和扭矩，这里需要将其修改为0，让其停下来 + stop(id); + DO(write_reg(id, kRegServoMinAngle, info->regDefaultVal_servoMinAngle)); + DO(write_reg(id, kRegServoMaxAngle, info->regDefaultVal_servoMaxAngle)); + return true; + } else { + int32_t val = 0; + int32_t nowmode = 0; + DO(read_reg(id, kRegServoRunMode, &nowmode)); + if (nowmode != 0) { + DO(write_reg(id, kRegServoRunMode, (uint8_t)0)); + } + DO(write_reg(id, kRegServoTorqueSwitch, 1)); + + return true; + } +} +bool FeiTeServoMotor::setInMode1(uint8_t id) { + zlock_guard l(m_mutex); + feite_servo_info_t* info = getServoInfo(id); + + if (!info) { + 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 { + stop(id); + int32_t nowmode = 0; + DO(read_reg(id, kRegServoRunMode, &nowmode)); + if (nowmode != 1) { + DO(write_reg(id, kRegServoRunMode, (uint8_t)1)); + } + return true; + } + return false; +} +bool FeiTeServoMotor::setInMode2(uint8_t id) { + zlock_guard l(m_mutex); + feite_servo_info_t* info = getServoInfo(id); + + if (!info) { + ZLOGE(TAG, "runInMode0 id:%d not find", id); + return false; + } + if (!info->support_mode2) { + ZLOGE(TAG, "runInMode2 id:%d not support", id); + return false; + } + return setInMode0(id); +} + bool FeiTeServoMotor::isSupportMode0(uint8_t id) { zlock_guard l(m_mutex); feite_servo_info_t* info = getServoInfo(id); @@ -268,6 +351,10 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed) ZLOGE(TAG, "runInMode1 id:%d not support", id); return false; } else { + if (info->shaft) { + speed = -speed; + } + if (limitTorque <= 0) limitTorque = 0; if (limitTorque > 1000) limitTorque = 1000; stop(id); @@ -282,6 +369,82 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed) } return false; } + +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::setCurPos(uint8_t id, int32_t pos3600) { + zlock_guard l(m_mutex); + feite_servo_info_t* info = getServoInfo(id); + + if (!info) { + ZLOGE(TAG, "runInMode0 id:%d not find", id); + return false; + } + if (info->isSCS) { + ZLOGE(TAG, "runInMode1 id:%d not support", id); + return false; + } else { + if (info->range != 4096) { + return false; + } + if (info->shaft) { + pos3600 = 3600 - pos3600; + } + int32_t motor_pos = pos3600 / 10.0 / info->angleConverCoefficient; + return setCurPos4096(id, motor_pos); + } +} + +bool FeiTeServoMotor::isSupportSetCurPos(uint8_t id) { + zlock_guard l(m_mutex); + feite_servo_info_t* info = getServoInfo(id); + + if (!info) { + ZLOGE(TAG, "runInMode0 id:%d not find", id); + return false; + } + if (info->isSCS) { + ZLOGE(TAG, "runInMode1 id:%d not support", id); + return false; + } else { + if (info->range != 4096) { + return false; + } + return true; + } +} + +bool FeiTeServoMotor::setCurPos4096(uint8_t id, int32_t targetpos4095) { + DO(setInMode0(id)); + + int32_t nowpos = 0; + int32_t nowposcalibration = 0; + DO(write_reg(id, feite::reg_add_e::kRegServoTorqueSwitch, 0)); + DO(write_reg(id, kRegServoCalibration, 0)); + + DO(read_reg(id, kRegServoCurrentPos, &nowpos)); + int16_t newcalibrate = getcalibrate(nowpos, targetpos4095); + DO(write_reg(id, kRegServoLockFlag, 0)); + DO(write_reg(id, kRegServoCalibration, newcalibrate)); + DO(write_reg(id, kRegServoLockFlag, 1)); + + DO(write_reg(id, kRegServoTargetPos, targetpos4095)); + DO(write_reg(id, feite::reg_add_e::kRegServoTorqueSwitch, 1)); + + return true; +} + bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) { zlock_guard l(m_mutex); feite_servo_info_t* info = getServoInfo(id); @@ -296,6 +459,10 @@ bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) { } if (info->isSCS) { + if (info->shaft) { + torque = -torque; + } + stop(id); DO(write_reg(id, kRegServoMinAngle, 0)); DO(write_reg(id, kRegServoMaxAngle, 0)); @@ -352,6 +519,17 @@ bool FeiTeServoMotor::getPos(uint8_t id, int32_t* pos) { // ZLOGI(TAG, "getPos id:%d pos:%d", id, *pos); *pos = (*pos) * info->angleConverCoefficient * 10 + 0.5; + + if (info->shaft) { + *pos = 3600 - *pos; + } + if (*pos < 0) { + *pos = 0; + } + if (*pos > 3600) { + *pos = 3600; + } + return true; } bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) { diff --git a/sdk/components/mini_servo_motor/feite_servo_motor.hpp b/sdk/components/mini_servo_motor/feite_servo_motor.hpp index 6e521df..33c233e 100644 --- a/sdk/components/mini_servo_motor/feite_servo_motor.hpp +++ b/sdk/components/mini_servo_motor/feite_servo_motor.hpp @@ -34,6 +34,8 @@ typedef struct { bool isSCS; + bool shaft; + bool bigend; bool support_mode0; // 角度伺服模式 bool support_mode1; // 速度闭环模式 @@ -43,6 +45,8 @@ typedef struct { int16_t regDefaultVal_servoMinAngle; int16_t regDefaultVal_servoMaxAngle; + int16_t range; + float angleConverCoefficient; } feite_servo_info_t; @@ -163,11 +167,16 @@ class FeiTeServoMotor { public: void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); - void regServo(uint8_t id, feite_servo_model_number_t model_type); + void regServo(uint8_t id, feite_servo_model_number_t model_type, bool shaft); feite_servo_info_t* getServoInfo(int id); bool ping(uint8_t id); bool stop(uint8_t id); + + bool setInMode0(uint8_t id); + bool setInMode1(uint8_t id); + bool setInMode2(uint8_t id); + /** * @brief * @@ -189,6 +198,10 @@ class FeiTeServoMotor { bool getPos(uint8_t id, int32_t* pos); bool isMove(uint8_t id, int32_t* move); + bool setCurPos(uint8_t id, int32_t pos); + bool isSupportSetCurPos(uint8_t id); + bool setCurPos4096(uint8_t id, int32_t pos); + public: bool write_reg(uint8_t id, int regadd, int32_t regval); bool read_reg(uint8_t id, int regadd, int32_t* regval); diff --git a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 4d4ca2b..5895fa8 100644 --- a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -3,15 +3,6 @@ using namespace iflytop; using namespace std; #define TAG "MiniServo" -static int32_t limit_input(int32_t input, int32_t min, int32_t max) { - int32_t ret = 0; - if (input > max) input = max; - if (input < min) input = min; - - ret = input; - return ret; -} - static bool poseq(int32_t a, int32_t b, int32_t err) { return abs(a - b) < err; } void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, config_t *cfg) { @@ -22,6 +13,7 @@ void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, u m_cfg = *cfg; m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal); + m_bus->setInMode0(idinbus); // 设置成模式0，这样能保证回读的位置是正确的。 mini_servo_enable(1); } void MiniServoCtrlModule::create_default_config(config_t &cfg) { @@ -94,6 +86,14 @@ int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) { int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) { ZLOGI(TAG, "%d mini_servo_move_to %d", m_module_id, pos3600); + // if (pos3600 > 3600 || pos3600 < 0) { + // return err::kparam_out_of_range; + // } + + // if (m_cfg.shaft != 0) { + // pos3600 = 3600 - pos3600; + // } + m_thread.stop(); if (!m_bus->ping(m_idinbus)) return err::ksubdevice_overtime; if (!m_state.enable) return err::kmini_servo_not_enable; @@ -185,6 +185,16 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t * return 0; } +int32_t MiniServoCtrlModule::mini_servo_set_cur_pos(int32_t pos) { + ZLOGI(TAG, "%d mini_servo_set_cur_pos %d", m_module_id, pos); + if (!m_bus->isSupportSetCurPos(m_idinbus)) { + return err::kmini_servo_mode_not_support; + } + bool suc = m_bus->setCurPos(m_idinbus, pos); + if (!suc) return err::ksubdevice_overtime; + return 0; +} + void MiniServoCtrlModule::befor_motor_move() { creg.module_status = 1; creg.module_errorcode = 0; diff --git a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index 3624368..a7eb9ce 100644 --- a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -7,10 +7,11 @@ namespace iflytop { class MiniServoCtrlModule : public ZIModule, public ZIMiniServo { - ENABLE_MODULE(StepMotorCtrlModule, kmini_servo_motor_module,PC_VERSION); + ENABLE_MODULE(StepMotorCtrlModule, kmini_servo_motor_module, PC_VERSION); public: typedef struct { + int32_t shaft; int32_t limit_velocity; // 0->10000 ,位置模式，有效。 int32_t limit_torque; // 0->1000 ,位置模式，速度模式，有效。 int32_t protective_torque; // 0->1000 ,暂时为使用 @@ -52,6 +53,7 @@ class MiniServoCtrlModule : public ZIModule, public ZIMiniServo { virtual int32_t mini_servo_set_mid_point() override; virtual int32_t mini_servo_read_io_state(int32_t ioindex, int32_t *val) override; + virtual int32_t mini_servo_set_cur_pos(int32_t pos) override; private: void befor_motor_move(); diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 4354741..7ad33cc 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -160,6 +160,7 @@ int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& if (!read) { module_active_cfg(); } + return ret; } int32_t XYRobotCtrlModule::__module_xxx_reg(int32_t param_id, bool read, int32_t& val) { @@ -560,4 +561,4 @@ int32_t XYRobotCtrlModule::xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2) *enc1 = m_stepM1->read_enc_val(); *enc2 = m_stepM2->read_enc_val(); return 0; -} \ No newline at end of file +} diff --git a/sdk/components/zcancmder/zcan_protocol_parser.cpp b/sdk/components/zcancmder/zcan_protocol_parser.cpp index 42029ea..d3e193e 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.cpp +++ b/sdk/components/zcancmder/zcan_protocol_parser.cpp @@ -53,6 +53,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(mini_servo_move_to); REGFN(mini_servo_rotate); REGFN(mini_servo_rotate_with_torque); + REGFN(mini_servo_set_cur_pos); REGFN(extboard_read_inio); REGFN(extboard_write_outio); @@ -413,6 +414,10 @@ int32_t ZCanProtocolParser::mini_servo_rotate_with_torque(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(1); return module->mini_servo_rotate_with_torque(cxt->params[0]); } +int32_t ZCanProtocolParser::mini_servo_set_cur_pos(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->mini_servo_set_cur_pos(cxt->params[0]); +} #undef MODULE_CLASS diff --git a/sdk/components/zcancmder/zcan_protocol_parser.hpp b/sdk/components/zcancmder/zcan_protocol_parser.hpp index 9c419dc..0a3bca0 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.hpp +++ b/sdk/components/zcancmder/zcan_protocol_parser.hpp @@ -79,14 +79,13 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(step_motor_get_subdevice_reg); CMDFN(step_motor_easy_reciprocating_motion); - - CMDFN(mini_servo_enable); CMDFN(mini_servo_read_pos); CMDFN(mini_servo_active_cfg); CMDFN(mini_servo_stop); CMDFN(mini_servo_set_mid_point); CMDFN(mini_servo_read_io_state); + CMDFN(mini_servo_set_cur_pos); CMDFN(mini_servo_move_to); CMDFN(mini_servo_rotate); diff --git a/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp b/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp index 6ec923e..a34016f 100644 --- a/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp +++ b/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp @@ -38,7 +38,7 @@ void Subboard30ShakeModule::initialize() { int subid = 4; int subidInBus = 1; - feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001); + feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001, true); module.create_default_config(cfg); module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg); @@ -51,7 +51,7 @@ void Subboard30ShakeModule::initialize() { int subid = 5; int subidInBus = 2; - feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001); + feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001, false); module.create_default_config(cfg); module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg); @@ -64,7 +64,7 @@ void Subboard30ShakeModule::initialize() { int subid = 6; int subidInBus = 3; - feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001); + feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001, false); module.create_default_config(cfg); module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg); @@ -77,7 +77,7 @@ void Subboard30ShakeModule::initialize() { int subid = 7; int subidInBus = 4; - feiteservomotor_bus.regServo(subidInBus, feite::kscs009); + feiteservomotor_bus.regServo(subidInBus, feite::kscs009, false); module.create_default_config(cfg); module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg); diff --git a/usrc/subboards/subboard90_optical_module/pri_board.h b/usrc/subboards/subboard90_optical_module/pri_board.h index 3a6c0fd..b4c58d5 100644 --- a/usrc/subboards/subboard90_optical_module/pri_board.h +++ b/usrc/subboards/subboard90_optical_module/pri_board.h @@ -92,8 +92,8 @@ #define MOTOR2_CSN PB1 #define MOTOR2_ENN PD4 -#define MOTOR2_REFL PD2 -#define MOTOR2_REFR PD8 +#define MOTOR2_REFL PD8 +#define MOTOR2_REFR PD2 #define MOTOR2_REFL_MIRROR true #define MOTOR2_REFR_MIRROR true @@ -102,7 +102,7 @@ #define MOTOR2_V1 800 #define MOTOR2_AMAX 300 -#define MOTOR2_MOTOR_SHAFT true +#define MOTOR2_MOTOR_SHAFT false #define MOTOR2_MOTOR_ONE_CIRCLE_PULSE 20 #define MOTOR2_MOTOR_ONE_CIRCLE_PULSE_DENOMINATOR 1 #define MOTOR2_STEPMOTOR_IHOLD 3