From 59ff433ed9deb4e045dcb479276ee9142f4e63db Mon Sep 17 00:00:00 2001 From: HSZ_HeSongZhen <210202959@qq.com> Date: Mon, 24 Mar 2025 14:24:37 +0800 Subject: [PATCH] =?UTF-8?q?1.=20=E4=BC=98=E5=8C=96=E5=9B=9E=E5=8E=9F?= =?UTF-8?q?=E7=82=B9=E5=8A=9F=E8=83=BD=202.=20=E5=A2=9E=E5=8A=A0=E4=B8=89?= =?UTF-8?q?=E8=89=B2=E7=81=AF=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/freertos.c | 3 - User/BSP/LED/exti_key_manager.cpp | 6 +- User/BSP/LED/led.c | 23 ++ User/BSP/LED/led.h | 39 ++- User/BSP/LED/pump_controller.cpp | 23 ++ User/BSP/LED/pump_controller.h | 2 + User/BSP/base/apphardware.cpp | 50 ++- User/BSP/base/apphardware.hpp | 4 +- User/BSP/can_control/can_config.h | 1 + User/BSP/can_control/can_protocol_parser.cpp | 4 +- User/BSP/status/elc_motor.cpp | 366 +++++----------------- User/BSP/status/elc_motor.h | 36 +-- User/BSP/status/elc_motor_helper.cpp | 12 + User/BSP/status/elc_motor_helper.h | 9 + User/BSP/status/motor_manager.cpp | 10 +- User/BSP/status/motor_manager.h | 1 - User/BSP/tim_pwm/pwm_control.cpp | 50 +-- User/BSP/uart_cmd/cmd_process_service_process.cpp | 89 +++--- User/BSP/uart_cmd/cmd_receive_service.c | 20 -- User/app/app_core.cpp | 44 +++ 20 files changed, 315 insertions(+), 477 deletions(-) diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index 17ae81a..445efe6 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -170,9 +170,6 @@ void feedDog(void) void StartIDLETask(void *argument) { for(;;) { - // LED_GREEN_TOGGLE; - // LED_RED_TOGGLE; - // LED_BLUE_TOGGLE; feedDog(); osDelay(2000); } diff --git a/User/BSP/LED/exti_key_manager.cpp b/User/BSP/LED/exti_key_manager.cpp index 32b9cd7..f2f9b47 100644 --- a/User/BSP/LED/exti_key_manager.cpp +++ b/User/BSP/LED/exti_key_manager.cpp @@ -129,7 +129,8 @@ void ExtiKeyManager::processKeyEvent(const uint16_t gpio_pin, const bool is_trig if (is_triggerd) { ZLOGI(TAG, "E_STOP_PIN Pressed"); iflytop::AppHardware::ins()->setE_Stop(true); - iflytop::AppHardware::ins()->SystemPowerOff(); + iflytop::AppHardware::ins()->SystemPowerOff(true); + tri_color_light(COLOR_RED); // 开启红灯 } else { ZLOGI(TAG, "E_STOP_PIN Released"); iflytop::AppHardware::ins()->setE_Stop(false); @@ -137,6 +138,9 @@ void ExtiKeyManager::processKeyEvent(const uint16_t gpio_pin, const bool is_trig if(isLaunched) { iflytop::AppHardware::ins()->SystemPowerOn(); } + else { + tri_color_light(COLOR_OFF); // 开启绿灯 + } } break; } diff --git a/User/BSP/LED/led.c b/User/BSP/LED/led.c index d9d7718..d9242a9 100644 --- a/User/BSP/LED/led.c +++ b/User/BSP/LED/led.c @@ -53,3 +53,26 @@ void LED_Init(void) // 初始化时将照明灯关闭 LIGHT_FLOOD_OFF; } + +void tri_color_light(const LightColor color) { + // 先关闭所有灯 + LED_GREEN_OFF; + LED_BLUE_OFF; + LED_RED_OFF; + + // 根据传入的枚举值开启相应的灯 + switch (color) { + case COLOR_GREEN: + LED_GREEN_ON; + break; + case COLOR_BLUE: + LED_BLUE_ON; + break; + case COLOR_RED: + LED_RED_ON; + break; + case COLOR_OFF: + default: + break; + } +} diff --git a/User/BSP/LED/led.h b/User/BSP/LED/led.h index 4fbdf50..8f95394 100644 --- a/User/BSP/LED/led.h +++ b/User/BSP/LED/led.h @@ -16,26 +16,26 @@ extern "C" { #define LED_KEY_ON HAL_GPIO_WritePin(LED_KEY_GPIO_Port, LED_KEY_Pin, GPIO_PIN_RESET) #define LED_KEY_TOGGLE do{ HAL_GPIO_TogglePin(LED_KEY_GPIO_Port, LED_KEY_Pin); }while(0) -// LED_GREEN -#define LED_GREEN_Pin GPIO_PIN_8 +// LED_GREEN（高电平点亮，低电平熄灭） +#define LED_GREEN_Pin GPIO_PIN_8 #define LED_GREEN_GPIO_Port GPIOE -#define LED_GREEN_OFF HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET) -#define LED_GREEN_ON HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET) -#define LED_GREEN_TOGGLE do{ HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin); }while(0) +#define LED_GREEN_ON HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET) // SET=高电平=ON +#define LED_GREEN_OFF HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET) // RESET=低电平=OFF +#define LED_GREEN_TOGGLE do { HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin); } while(0) -// LED_RED -#define LED_RED_Pin GPIO_PIN_9 +// LED_RED（高电平点亮，低电平熄灭） +#define LED_RED_Pin GPIO_PIN_9 #define LED_RED_GPIO_Port GPIOE -#define LED_RED_OFF HAL_GPIO_WritePin(LED_RED_GPIO_Port, LED_RED_Pin, GPIO_PIN_SET) -#define LED_RED_ON HAL_GPIO_WritePin(LED_RED_GPIO_Port, LED_RED_Pin, GPIO_PIN_RESET) -#define LED_RED_TOGGLE do{ HAL_GPIO_TogglePin(LED_RED_GPIO_Port, LED_RED_Pin); }while(0) +#define LED_RED_ON HAL_GPIO_WritePin(LED_RED_GPIO_Port, LED_RED_Pin, GPIO_PIN_SET) +#define LED_RED_OFF HAL_GPIO_WritePin(LED_RED_GPIO_Port, LED_RED_Pin, GPIO_PIN_RESET) +#define LED_RED_TOGGLE do { HAL_GPIO_TogglePin(LED_RED_GPIO_Port, LED_RED_Pin); } while(0) -// LED_BLUE -#define LED_BLUE_Pin GPIO_PIN_10 +// LED_BLUE（高电平点亮，低电平熄灭） +#define LED_BLUE_Pin GPIO_PIN_10 #define LED_BLUE_GPIO_Port GPIOE -#define LED_BLUE_ON HAL_GPIO_WritePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin, GPIO_PIN_RESET) -#define LED_BLUE_OFF HAL_GPIO_WritePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin, GPIO_PIN_SET) -#define LED_BLUE_TOGGLE do{ HAL_GPIO_TogglePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin); }while(0) +#define LED_BLUE_ON HAL_GPIO_WritePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin, GPIO_PIN_SET) +#define LED_BLUE_OFF HAL_GPIO_WritePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin, GPIO_PIN_RESET) +#define LED_BLUE_TOGGLE do { HAL_GPIO_TogglePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin); } while(0) // BEPPER_ALARM #define BEPPER_ALARM_Pin GPIO_PIN_11 @@ -66,10 +66,17 @@ extern "C" { #define RK3588_POWER_ON HAL_GPIO_WritePin(RK3588_POWER_GPIO_Port, RK3588_POWER_Pin, GPIO_PIN_RESET) #define RK3588_POWER_TOGGLE do{ HAL_GPIO_TogglePin(RK3588_POWER_GPIO_Port, RK3588_POWER_Pin); }while(0) - +typedef enum { + COLOR_GREEN, + COLOR_BLUE, + COLOR_RED, + COLOR_OFF +} LightColor; void LED_Init(void); +void tri_color_light(const LightColor color); + #ifdef __cplusplus } #endif diff --git a/User/BSP/LED/pump_controller.cpp b/User/BSP/LED/pump_controller.cpp index a94376f..ff7280e 100644 --- a/User/BSP/LED/pump_controller.cpp +++ b/User/BSP/LED/pump_controller.cpp @@ -65,6 +65,29 @@ void PumpController::setFlowSpeed(double flow) { this->freq_ = is_forward ? 1 : -1; ZLOGI("【PUMP】", "[ZERO LIMIT] FREQ [%d] ", freq_); } + + ZLOGI("【PUMP】", "FINAL FREQ [%d] ", freq_); + if(this->isPowerOn) { + this->openPwm(); + this->powerOn(); + } +} + +void PumpController::moveWithFlowSpeed(double flow) { + // 设置为零 自动关闭 + if(flow == 0.0) { + this->powerOff(); + ZLOGI("【PUMP】", "SET FLOW POWER OFF"); + return; + } + bool is_forward = flow > 0; + + const double abs_flow = fabs(flow); + this->freq_ = calculateFrequency(abs_flow); + if(this->freq_ == 0) { + this->freq_ = is_forward ? 1 : -1; + ZLOGI("【PUMP】", "[ZERO LIMIT] FREQ [%d] ", freq_); + } ZLOGI("【PUMP】", "FINAL FREQ [%d] ", freq_); this->setDirection(is_forward); this->openPwm(); diff --git a/User/BSP/LED/pump_controller.h b/User/BSP/LED/pump_controller.h index cfb72db..2e9e866 100644 --- a/User/BSP/LED/pump_controller.h +++ b/User/BSP/LED/pump_controller.h @@ -45,6 +45,8 @@ public: // 设置注射泵流速 void setFlowSpeed(double flow); + void moveWithFlowSpeed(double flow); + // 设备是否正常 bool isNoraml(); diff --git a/User/BSP/base/apphardware.cpp b/User/BSP/base/apphardware.cpp index c2b6bbe..ab6c25b 100644 --- a/User/BSP/base/apphardware.cpp +++ b/User/BSP/base/apphardware.cpp @@ -46,6 +46,7 @@ void AppHardware::initialize() { TMC51X0Cfg tmc5130cfg1 = {&MOTOR_SPI_INS, MOTO1_CSN_IO, MOTO1_DRV_ENN_IO}; MOTO1.initialize(0, tmc5130cfg1); ZLOGI(TAG, "motor1 initialize TMC51X0: %02X", MOTO1.readICVersion()); + MOTO1.enable(false); #if 0 MOTO1.setIHOLD_IRUN(0, 7, 100); #else @@ -58,7 +59,7 @@ void AppHardware::initialize() { TMC51X0Cfg tmc5130cfg2 = {&MOTOR_SPI_INS, MOTO2_CSN_IO, MOTO2_DRV_ENN_IO}; MOTO2.initialize(1, tmc5130cfg2); ZLOGI(TAG, "motor2 initialize TMC51X0: %02X", MOTO2.readICVersion()); - + MOTO2.enable(false); #if 0 MOTO2.setIHOLD_IRUN(0, 10, 100); #else @@ -71,10 +72,18 @@ void AppHardware::initialize() { TMC51X0Cfg tmc5130cfg3 = {&MOTOR_SPI_INS, MOTO3_CSN_IO, MOTO3_DRV_ENN_IO}; MOTO3.initialize(2, tmc5130cfg3); ZLOGI(TAG, "motor3 initialize TMC51X0: %02X", MOTO3.readICVersion()); + MOTO3.enable(false); MOTO3.setIHOLD_IRUN(0, 5, 100); MOTO3.setScale(1000); MOTO3.setEncResolution(-1000); + MOTO1.enable(true); + osDelay(10); + MOTO2.enable(true); + osDelay(10); + MOTO3.enable(true); + osDelay(10); + // GPIO 初始化 AirValve::initGPIO(); three_way_valve.initGPIO(); @@ -102,13 +111,6 @@ void AppHardware::initialize() { AppHardware::ins()->uart_control.start(); // 启动CAN ZLOGI(TAG, "===== UART Controller Start Suc"); - AppHardware::ins()->MOTO1.enable(true); - osDelay(10); - AppHardware::ins()->MOTO2.enable(true); - osDelay(10); - AppHardware::ins()->MOTO3.enable(true); - osDelay(10); - isLaunched_ = false; isStarted_ = false; } @@ -119,28 +121,28 @@ void AppHardware::SystemPowerOn() { if(isStarted_) { return; } - LED_GREEN_OFF; // 开启绿灯 + tri_color_light(COLOR_GREEN); // 开启绿灯 LIGHT_FLOOD_ON; // 照明灯开 - - - osDelay(200); - + osDelay(10); ZLOGI(TAG, "======================= System Start ======================="); isStarted_ = true; } -void AppHardware::SystemPowerOff() { - if(!isStarted_) { - return; +void AppHardware::SystemPowerOff(bool is_force) { + if(!is_force) { + if(!isStarted_) { + return; + } } - LED_GREEN_ON; // 关灯 + + tri_color_light(COLOR_OFF); // 关灯 LIGHT_FLOOD_OFF; // 照明灯关闭 - AppHardware::ins()->high_voltage_pack.turnOffPower(); - AppHardware::ins()->pump_controller.powerOff(); - AppHardware::ins()->three_way_valve.setMode(ThreeWayValve::ON_C); - AppHardware::ins()->laser_control.powerOff(); + high_voltage_pack.turnOffPower(); + pump_controller.powerOff(); + three_way_valve.setMode(ThreeWayValve::ON_C); + laser_control.powerOff(); AirValve::closeCleaning(); AirValve::closeDehumidification(); @@ -153,12 +155,6 @@ void AppHardware::SystemPowerOff() { MotorManager::ins()->motors[2].runE_Stop(); osDelay(10); -#if 0 - AppHardware::ins()->MOTO1.enable(false); - AppHardware::ins()->MOTO2.enable(false); - AppHardware::ins()->MOTO3.enable(false); -#endif - osDelay(200); ZLOGI(TAG, "======================= System Shut Down ======================="); isStarted_ = false; } diff --git a/User/BSP/base/apphardware.hpp b/User/BSP/base/apphardware.hpp index e3b69ae..8a7c4b0 100644 --- a/User/BSP/base/apphardware.hpp +++ b/User/BSP/base/apphardware.hpp @@ -73,7 +73,7 @@ class AppHardware { ThreeWayValve three_way_valve; // 三通阀控制 PumpController pump_controller; // 电机泵 HighVoltagePack high_voltage_pack; // 注射泵 - LaserControl laser_control; // 激光 + LaserControl laser_control; // 激光 bool hardwareInitedOK = false; @@ -97,7 +97,7 @@ class AppHardware { void SystemPowerOn(); - void SystemPowerOff(); + void SystemPowerOff(bool is_force = false); void setTJCScreenInDownloadMode(); diff --git a/User/BSP/can_control/can_config.h b/User/BSP/can_control/can_config.h index 0639e1a..9ea4b7d 100644 --- a/User/BSP/can_control/can_config.h +++ b/User/BSP/can_control/can_config.h @@ -95,6 +95,7 @@ #define HIGH_VOLTAGE_PACK_SET_ADDRESS 0x20 // 高压电包设置寄存器 #define PUMP_DRIVER_SPEED_ADDRESS 0x21 // 注射泵速度 ML #define LASER_POWER_ADDRESS 0x22 // 激光控制 +#define PUMP_DRIVER_SET_SPEED_ADDRESS 0x23 // 设置注射泵速度 ML // 主动推送事件 功能码 #define UPLOAD_FUN_CODE_MOTOR_X 0x5A diff --git a/User/BSP/can_control/can_protocol_parser.cpp b/User/BSP/can_control/can_protocol_parser.cpp index dd5d77f..6e9851a 100644 --- a/User/BSP/can_control/can_protocol_parser.cpp +++ b/User/BSP/can_control/can_protocol_parser.cpp @@ -434,7 +434,7 @@ CanMessage CanProtocolParser::handle_write_multi_regsiter(uint8_t seq_id, uint32 case PUMP_DRIVER_SPEED_ADDRESS: { const int32_t i_flow = shortArrayToLittleEndianInt(arr); const double d_flow = i_flow * 0.01; - AppHardware::ins()->pump_controller.setFlowSpeed(d_flow); + AppHardware::ins()->pump_controller.moveWithFlowSpeed(d_flow); ZLOGI(TAG, "SET PUMP SPEED ADDRESS %d", i_flow); break;; } @@ -484,7 +484,7 @@ CanMessage CanProtocolParser::handle_read_coils(uint8_t seq_id, uint32_t frame_i const bool isAtOrigin = true; const bool isAxisLimit = true; #else - const bool isMove = MotorManager::ins()->motors[motor_index].isMoving(); + const bool isMove = MotorManager::ins()->motors[motor_index].isMoveFinished(); const bool isAtOrigin = ExtiKeyManager::ins()->isPinTriggered(getORIGINPin(motor_index)); const bool isAxisLimit = ExtiKeyManager::ins()->isPinTriggered(getAxisLimitPin(motor_index)); #endif diff --git a/User/BSP/status/elc_motor.cpp b/User/BSP/status/elc_motor.cpp index f84f202..e5a52ae 100644 --- a/User/BSP/status/elc_motor.cpp +++ b/User/BSP/status/elc_motor.cpp @@ -29,25 +29,14 @@ void ECLMotor::readTimerCallback(void *arg) { // 读取电机位置 void ECLMotor::readMotorPosition() { // 清除标志位 -#if 0 - if(isHoming()) { - if(++home_count_ == max_home_count_) { - // 强制清零 - home_count_ = 0; - homeState_ = STATE_INIT; - ZLOGI(TAG, "【HOME STATE COUNT FORCE CLEAR】"); - } - } -#endif - TMC51X0 *motor = AppHardware::ins()->getPump(index_); if (motor == nullptr) { return; } - bool is_reached = false; - bool is_move_finished = false; - + osMutexAcquire(mutex_, osWaitForever); + bool last_is_move_finished = is_move_finished_; + osMutexRelease(mutex_); int32_t x_actual = 0; int32_t enc_val = 0; @@ -58,76 +47,31 @@ void ECLMotor::readMotorPosition() { rt_speed = motor->getVACTUAL(); // 获取实时速度 TMC5130RampStat ramp_stat = motor->getRampStat(); - is_reached = motor->isReachTarget(&ramp_stat); + bool is_move_finished = motor->isReachTarget(&ramp_stat); int32_t postion = 0; - int32_t distance = 0; if (0 == enc_val) { postion = x_actual; } else { postion = enc_val; } - distance = abs(target_pos_ - postion); - if (distance < 50 && is_reached) { - is_move_finished = true; - } - - if (is_reached) { - is_move_finished = true; - } - -#if 1 -#else - if(is_moving) - { - if(0 == enc_val) { - postion = x_actual; - } - else { - postion = enc_val; - } - - const bool is_forward = target_pos_ - postion > 0; - distance = abs(target_pos_ - postion); - - // 检查目标距离与实际距离是否一致 - if(distance < 20 || is_forward != isFoward_) { - // 电机移动停止 - motor->stop(); - // 发送CAN 消息 - is_moving = false; - this->runStop(); - ZLOGI(TAG, "MOTOR DETECT STOP!!! "); -#if TIMER_IS_FINE - setReadRate(READ_RATE_LOW); -#endif - - } - } -#endif - bool is_moving = false; // 移动中 osMutexAcquire(mutex_, osWaitForever); - is_moving = is_moving_; - if (is_move_finished) { - is_moving_ = false; - } + is_move_finished_ = is_move_finished; x_actual_ = x_actual; enc_val_ = enc_val; rt_speed_ = rt_speed; osMutexRelease(mutex_); - if (is_moving && is_move_finished) { + if (!last_is_move_finished && is_move_finished) { //处理移动结束的问题 -#if 1 - if (isHoming()) { - ZLOGI(TAG, "【[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED】"); -#if 1 + if (!isHomeSuc()) { + ZLOGI(TAG, "[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED"); + osMutexAcquire(mutex_, osWaitForever); home_event_.event_type = EVENT_MOTOR_MOVE_FINISHED; + osMutexRelease(mutex_); osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); -#endif } -#endif // 发送CAN 消息 { #if CAN_MODULE_ENABLE @@ -140,25 +84,8 @@ void ECLMotor::readMotorPosition() { } // 设置读取速率 -void ECLMotor::setReadRate(ReadRate rate) { - readRate = rate; - uint32_t period; - switch (rate) { - case READ_RATE_LOW: - period = 500; // 1秒 - break; - case READ_RATE_MEDIUM: - period = 100; // 500毫秒 - break; - case READ_RATE_HIGH: - period = 50; // 50毫秒 - break; - default: - period = 1000; - break; - } - osTimerStop(readTimer); - osTimerStart(readTimer, period); +void ECLMotor::startReadTimer() { + osTimerStart(readTimer, 50); // // 50毫秒 } ECLMotor::ECLMotor() { @@ -173,16 +100,14 @@ ECLMotor::ECLMotor() { x_actual_ = 0; target_pos_ = 0; target_speed_ = 10; // 10 / 60 * 6s ==== 6 S 可以转一圈 - is_moving_ = 0; + is_move_finished_ = false; enc_val_ = 0; - isHomed_ = false; - homeState_ = STATE_INIT; + is_home_init_ = false; + is_home_suc_ = false; + home_state_ = STATE_INIT; isRunHomingTask = true; - // 初始读取速率 - readRate = READ_RATE_HIGH; - // 创建定时器 osTimerAttr_t timerAttr = { .name = "MotorReadTimer", @@ -212,9 +137,8 @@ ECLMotor::ECLMotor() { ZLOGE(TAG, "THREAD INDEX %d Create Failed", index_); } - // 启动定时器 - setReadRate(readRate); + startReadTimer(); } ECLMotor::~ECLMotor() { @@ -251,53 +175,7 @@ void ECLMotor::setTargetPosition(int32_t target) { motor->moveTo(target, this->target_speed_); osMutexAcquire(mutex_, osWaitForever); target_pos_ = target; - is_moving_ = true; osMutexRelease(mutex_); - -#if 0 - TMC51X0 *motor = AppHardware::ins()->getPump(index_); - if(motor == nullptr) { - return; - } - if(is_Flip[index_]) { - target = -target; - ZLOGI(TAG, "【MOTOR 【%d】TARGET FLIP】", index_); - } - - osMutexAcquire(mutex_, osWaitForever); - int32_t postion = 0; - -#if 0 - if(0 == enc_val_) { - postion = x_actual_; - } - else { - postion = enc_val_; - } -#else - if(0 == enc_val_) { - postion = enc_val_; - } - else { - postion = x_actual_; - } -#endif - - const bool is_forward = target - postion > 0; - isFoward_ = is_forward; - target_pos_ = target; - is_moving_ = true; - osMutexRelease(mutex_); -#if TIMER_IS_FINE - this->setReadRate(READ_RATE_HIGH); -#endif - - int32_t abs_speed = abs(speed_); - int32_t speed = is_forward ? abs_speed : -abs_speed; - - // 运行 - this->ECL_Rotate(speed, false); -#endif } void ECLMotor::setShiftPosition(int32_t shift_target) { @@ -313,7 +191,6 @@ void ECLMotor::setShiftPosition(int32_t shift_target) { osMutexAcquire(mutex_, osWaitForever); target_pos_ = target_pos_ + shift_target; - is_moving_ = true; osMutexRelease(mutex_); } @@ -330,7 +207,6 @@ void ECLMotor::moveToWithSpeed(int32_t target, int32_t speed) { osMutexAcquire(mutex_, osWaitForever); target_pos_ = target; - is_moving_ = true; osMutexRelease(mutex_); } @@ -343,15 +219,10 @@ void ECLMotor::setSpeed(int32_t speed) { } osMutexAcquire(mutex_, osWaitForever); this->target_speed_ = speed; -#if 0 - if(is_moving_) { - motor->moveTo(target_pos_, this->target_speed_); - } -#else - if (is_moving_) { + // 当前正在移动 + if (!is_move_finished_) { motor->setVMax(this->target_speed_); } -#endif osMutexRelease(mutex_); } @@ -364,11 +235,11 @@ int32_t ECLMotor::getRTSpeed() { } // 获取电机运动状态 -bool ECLMotor::isMoving() { +bool ECLMotor::isMoveFinished() { osMutexAcquire(mutex_, osWaitForever); - bool moving = is_moving_; + bool is_move_finshed = is_move_finished_; osMutexRelease(mutex_); - return moving; + return is_move_finshed; } // 获取编码器位置 @@ -386,24 +257,19 @@ void ECLMotor::setEncoderPosition(int32_t encoderPos) { osMutexRelease(mutex_); } -// 新增设置是否运动的接口 -void ECLMotor::setMoving(bool moving) { - osMutexAcquire(mutex_, osWaitForever); - is_moving_ = moving; - osMutexRelease(mutex_); -} - void ECLMotor::setMotorIndex(int32_t index) { this->index_ = index; } -bool ECLMotor::isHomed() { - return isHomed_; +bool ECLMotor::isHomeInit() { + return is_home_init_; } -bool ECLMotor::isHoming() { - bool isDoHome = homeState_ != STATE_INIT; - return isDoHome; +bool ECLMotor::isHomeSuc() { + osMutexAcquire(mutex_, osWaitForever); + bool is_home_suc = is_home_suc_; + osMutexRelease(mutex_); + return is_home_suc; } void ECLMotor::runhomeSuc() { @@ -414,10 +280,16 @@ void ECLMotor::runhomeSuc() { // 发送 CAN 信息 motor->setEncVal(0); motor->setXACTUAL(0); - this->isHomed_ = true; - this->homeState_ = STATE_INIT; - ZLOGI(TAG, "【HOMED】 MOVE TO HOME SUC"); { + // 置标志位 + osMutexAcquire(mutex_, osWaitForever); + this->is_home_init_ = true; + this->is_home_suc_ = true; + this->home_state_ = STATE_INIT; + osMutexRelease(mutex_); + + ZLOGI(TAG, "【 MOVE TO HOME SUC 】"); + { #if CAN_MODULE_ENABLE CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Home_Finished, index_); AppHardware::ins()->can0Controller.sendMessage(msg); @@ -426,32 +298,27 @@ void ECLMotor::runhomeSuc() { } void ECLMotor::startMoveHome() { - // 避免有相同的操作 - bool isDoHoming = this->isHoming(); - if (isDoHoming) { - ZLOGI(TAG, "MOTOR [%d] HOME FAILED, Current is Homing", this->index_); - // return; - } - home_count_ = 0; // 开始前清零 - bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_); - ZLOGI(TAG, "START NOT IN HOME") + home_start_tick_ = 0; + + osMutexAcquire(mutex_, osWaitForever); + this->is_home_suc_ = false; + osMutexRelease(mutex_); + + const bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_); // 当前是否在原点 if (isAtOrigin) { home_event_.event_type = EVENT_IN_ORIGIN; - ZLOGI(TAG, "EVENT_IN_ORIGIN"); + ZLOGI(TAG, "[PUT] EVENT IN ORIGIN"); osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); if (status != osOK) { - ZLOGE(TAG, "EVENT_IN_ORIGIN SEND FAIL, status: %d", status); + ZLOGE(TAG, "EVENT IN ORIGIN SEND FAIL, status: %d", status); } } else { - // if (homeState_ == STATE_INIT) - { - home_event_.event_type = EVENT_START_HOMING; - ZLOGI(TAG, "EVENT_START_HOMING"); - osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); - if (status != osOK) { - ZLOGE(TAG, "EVENT_START_HOMING SEND FAIL, status: %d", status); - } + home_event_.event_type = EVENT_START_HOMING; + ZLOGI(TAG, "[PUT] EVENT_START_HOMING"); + osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); + if (status != osOK) { + ZLOGE(TAG, "EVENT_START_HOMING SEND FAIL, status: %d", status); } } } @@ -461,7 +328,7 @@ void ECLMotor::setHomeSeqId(uint8_t seq_id) { } void ECLMotor::setMoveFinishSeqId(uint8_t seq_id) { - this->seq_id_move_finished_ = seq_id; + this->seq_id_move_ = seq_id; } void ECLMotor::homingTask(void *arg) { @@ -475,32 +342,27 @@ void ECLMotor::homingTask(void *arg) { if (motor == nullptr) { return; } - - ZLOGI(TAG, "HANDLE NOT HOMED"); #if 1 switch (home_event.event_type) { case EVENT_START_HOMING: { - ZLOGI(TAG, "[]EVENT_START_HOMING"); - if (eclMotor->homeState_ == STATE_INIT) { - eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_MIDDLE; + osMutexAcquire(eclMotor->mutex_, osWaitForever); + eclMotor->home_state_ = STATE_BACK_TO_ORIGIN_MIDDLE; + osMutexRelease(eclMotor->mutex_); // 中速倒转 int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); int absSpeed = abs(speed); eclMotor->ECL_Rotate(-absSpeed); - ZLOGI(TAG, "【HOME】 BACK TO ORIGIN MIDDLE"); - } break; } case EVENT_ORIGIN_ENTER: { - // ZLOGI(TAG, "[]EVENT_ORIGIN_ENTER"); // if first enter - if (eclMotor->homeState_ == STATE_BACK_TO_ORIGIN_MIDDLE) { + if(eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_MIDDLE) { // 停止 // 中速向前10mm - // int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); - // eclMotor->ECL_Rotate(speed); - eclMotor->homeState_ = STATE_LEAVE_ORIGIN_MIDDLE; + osMutexAcquire(eclMotor->mutex_, osWaitForever); + eclMotor->home_state_ = STATE_LEAVE_ORIGIN_MIDDLE; + osMutexRelease(eclMotor->mutex_); // 位置清零 motor->setEncVal(0); @@ -509,12 +371,10 @@ void ECLMotor::homingTask(void *arg) { const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed); // 4000 4 圈 - ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] FORWARD TO ORIGIN MIDDLE"); } - // if second enter - if (eclMotor->homeState_ == STATE_BACK_TO_ORIGIN_LOW) { + if (eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_LOW) { // 位置清零 // 电机停止 // 回归原点成功 @@ -522,54 +382,29 @@ void ECLMotor::homingTask(void *arg) { } break; } - case EVENT_ENCODER_NEAR_ORIGN: { - // ZLOGI(TAG, "[]EVENT_ENCODER_NEAR_ORIGN"); - if (eclMotor->homeState_ == STATE_LEAVE_ORIGIN_MIDDLE) { - // 停止 - // 低速回归 - eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_LOW; - - int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_); - eclMotor->ECL_Rotate(-speed); - ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW"); - } - break; - } case EVENT_IN_ORIGIN: { - // ZLOGI(TAG, "[]EVENT_IN_ORIGIN"); - if (eclMotor->homeState_ == STATE_INIT) { - // 中速向前10mm - eclMotor->homeState_ = STATE_FORWARD_TO_ORIGIN_MIDDLE; - - int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); - eclMotor->ECL_Rotate(speed); - - ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE"); - } - break; - } - case EVENT_ORIGIN_LEAVE: { - // ZLOGI(TAG, "[]EVENT_ORIGIN_LEAVE"); - if (eclMotor->homeState_ == STATE_FORWARD_TO_ORIGIN_MIDDLE) { - // 停止 前向移动 1mm - eclMotor->homeState_ = STATE_LEAVE_ORIGIN_MIDDLE; - motor->stop(); - // 位置清零 - motor->setEncVal(0); - motor->setXACTUAL(0); - - const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); - eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed); - - ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE"); - } + ZLOGI(TAG, "[]EVENT IN ORIGIN"); + // 中速向前10mm + motor->setEncVal(0); + motor->setXACTUAL(0); + + // 前向移动 1mm + osMutexAcquire(eclMotor->mutex_, osWaitForever); + eclMotor->home_state_ = STATE_LEAVE_ORIGIN_MIDDLE; + osMutexRelease(eclMotor->mutex_); + + const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); + eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed); + ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE"); break; } case EVENT_MOTOR_MOVE_FINISHED: { - ZLOGI(TAG, "[]EVENT_MOTOR_MOVE_FINISHED"); - if (eclMotor->homeState_ == STATE_LEAVE_ORIGIN_MIDDLE) { + ZLOGI(TAG, "[]EVENT MOTOR MOVE FINISHED"); + if (eclMotor->home_state_ == STATE_LEAVE_ORIGIN_MIDDLE) { // 低速回归 - eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_LOW; + osMutexAcquire(eclMotor->mutex_, osWaitForever); + eclMotor->home_state_ = STATE_BACK_TO_ORIGIN_LOW; + osMutexRelease(eclMotor->mutex_); const int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_); eclMotor->ECL_Rotate(-speed); ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW"); @@ -583,39 +418,17 @@ void ECLMotor::homingTask(void *arg) { } else { ZLOGI(TAG, "GET MESSAGE FAILED"); } - ZLOGI(TAG, "RUN MOTOR [END] %d", eclMotor->index_); } } void ECLMotor::moveToHome() { -#if 0 - TMC51X0 *motor = AppHardware::ins()->getPump(index_); - if (motor == nullptr) { - return; - } - osMutexAcquire(mutex_, osWaitForever); - is_moving_ = false; - osMutexRelease(mutex_); - - int32_t abs_speed = abs(target_speed_); - - int32_t roback_speed = -abs_speed; -#if TIMER_IS_FINE - this->setReadRate(READ_RATE_HIGH); -#endif - - // 倒转回HOME - this->ECL_Rotate(roback_speed); - ZLOGI(TAG, "ECL MOTOR HOME ROBACK SPEED %d", roback_speed); -#else this->startMoveHome(); -#endif } void ECLMotor::runZeroLimit(bool isEnter) { // 处理回home 位逻辑 - if (isHoming()) { + if (!isHomeSuc()) { TMC51X0 *motor = AppHardware::ins()->getPump(index_); if (motor == nullptr) { return; @@ -632,7 +445,6 @@ void ECLMotor::runZeroLimit(bool isEnter) { } osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); - return; } @@ -645,13 +457,12 @@ void ECLMotor::runZeroLimit(bool isEnter) { return; } motor->stop(); - osDelay(10); { - CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_finished_, Motor_Move_Finished, index_); + { #if CAN_MODULE_ENABLE + CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_, Motor_Move_Finished, index_); AppHardware::ins()->can0Controller.sendMessage(msg); #endif } - osDelay(10); // 重置编码器 和 位置 motor->setXACTUAL(0); motor->setEncVal(0); @@ -659,7 +470,6 @@ void ECLMotor::runZeroLimit(bool isEnter) { this->x_actual_ = 0; this->enc_val_ = 0; this->target_pos_ = 0; - this->is_moving_ = false; osMutexRelease(mutex_); ZLOGI("ECL", "MOTOR [%d] runZeroLimit", this->index_); } @@ -674,6 +484,10 @@ void ECLMotor::runEndLimit(bool isEnter) { void ECLMotor::runE_Stop() { // 发送CAN 消息 + osMutexAcquire(mutex_, osWaitForever); + this->is_home_suc_ = false; + osMutexRelease(mutex_); + this->runStop(); } @@ -693,14 +507,9 @@ void ECLMotor::runStop() { CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Move_Finished, index_); AppHardware::ins()->can0Controller.sendMessage(msg); #endif - - osMutexAcquire(mutex_, osWaitForever); - this->is_moving_ = false; - osMutexRelease(mutex_); } void ECLMotor::ECL_Rotate(int32_t speed, const bool is_flip) { -#ifdef TEST TMC51X0 *motor = AppHardware::ins()->getPump(index_); if (motor == nullptr) { return; @@ -713,7 +522,4 @@ void ECLMotor::ECL_Rotate(int32_t speed, const bool is_flip) { motor->rotate(speed); ZLOGI(TAG, "ECL MOTOR %d SPEED %d", index_, speed); -#else - motor->rotate(roback_speed); -#endif } diff --git a/User/BSP/status/elc_motor.h b/User/BSP/status/elc_motor.h index 9136f2b..39c1462 100644 --- a/User/BSP/status/elc_motor.h +++ b/User/BSP/status/elc_motor.h @@ -58,27 +58,28 @@ private: int32_t rt_speed_; // 实时速度 int32_t target_pos_; // 目标位置 int32_t target_speed_; // 目标速度 - bool is_moving_; + bool is_move_finished_; // 是否已经移动结束 bool isFoward_; // 是否正向运动 - ReadRate readRate; osTimerId_t readTimer; osMutexId_t mutex_; - - bool isHomed_{ false }; // 是否已经回过原点 - HomeState homeState_{STATE_INIT}; // home 状态机 - HomeEvent home_event_; - int32_t home_count_ { 0 }; // 回 home 点计数 超时清零 - int32_t max_home_count_ { 600 }; // 60S == 100ms * 600 - DistanceLevel level_ { DistanceLow }; bool isRunHomingTask { true }; // 是否运行回原点任务 - osMessageQueueId_t homeEventQueue; // 回原点事件队列 osThreadAttr_t home_task_attr = {0}; osThreadId_t homingStateMachineThreadId; // 回原点线程 + HomeEvent home_event_; + osMessageQueueId_t homeEventQueue; // 回原点事件队列 + HomeState home_state_{STATE_INIT}; // home 状态机 + DistanceLevel level_ { DistanceLow }; + + int32_t home_start_tick_ { 0 }; // 回 home 点计数 超时清零 + int32_t max_home_tick_ { 60 * 1000 }; // 60 s 后自动重置 home 状态 + + bool is_home_init_{ false }; // 是否已经第一次（初始化回过原点）回过原点 + bool is_home_suc_ { false }; // 是否已经成功回原点 - uint8_t seq_id_home_ {0}; // 回 HOME 点 SEQ ID - uint8_t seq_id_move_finished_ {0}; // 运动到指定位置 点 SEQ ID + uint8_t seq_id_home_ {0}; // 回 HOME 点 指令 SEQ ID + uint8_t seq_id_move_ {0}; // 移动到指定到位置 SEQ ID static void readTimerCallback(void* arg); public: @@ -87,10 +88,10 @@ public: void setMotorIndex(int32_t index); // 是否已经回归到原点 - bool isHomed(); + bool isHomeInit(); // 是否正在回原点的逻辑 - bool isHoming(); + bool isHomeSuc(); void runhomeSuc(); @@ -108,11 +109,9 @@ public: void setSpeed(int32_t speed); - bool isMoving(); + bool isMoveFinished(); int32_t getEncoderPosition(); - void setMoving(bool moving); // 新增设置是否运动的接口 - void moveToHome(); void runZeroLimit(bool isEnter); // 停止运动 @@ -123,9 +122,8 @@ public: void ECL_Rotate(int32_t speed, const bool is_flip = true); - void readMotorPosition(); - void setReadRate(ReadRate rate); + void startReadTimer(); void setEncoderPosition(int32_t encoderPos); diff --git a/User/BSP/status/elc_motor_helper.cpp b/User/BSP/status/elc_motor_helper.cpp index 7f8862b..e6af32a 100644 --- a/User/BSP/status/elc_motor_helper.cpp +++ b/User/BSP/status/elc_motor_helper.cpp @@ -13,6 +13,7 @@ const int32_t ELCMotorHelper::rEncPosLimitArray[MOTOR_NUM] = { (int32_t)(MOTOR_Z_MAX_DISTANCE * MOTOR_Z_MM_PER_R * 1000) }; +#if MOTOR_REAL const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { 600, 600, 600 }; @@ -22,6 +23,17 @@ const int32_t ELCMotorHelper::rPerMinMiddleSpeedArray[MOTOR_NUM] = { const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = { 60, 30, 30 }; +#else +const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { + 600, 600, 600 +}; +const int32_t ELCMotorHelper::rPerMinMiddleSpeedArray[MOTOR_NUM] = { + 200, 200, 200 +}; +const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = { + 30, 30, 30 +}; +#endif const int32_t ELCMotorHelper::encLockRotorTH[MOTOR_NUM] = {500, 500, 500}; // 电机差半圈 diff --git a/User/BSP/status/elc_motor_helper.h b/User/BSP/status/elc_motor_helper.h index 17c572d..94972ba 100644 --- a/User/BSP/status/elc_motor_helper.h +++ b/User/BSP/status/elc_motor_helper.h @@ -6,17 +6,26 @@ #define ELC_MOTOR_HELPER_H #include +#define MOTOR_REAL 0 + /** * 电机数量 */ #define MOTOR_NUM 3 + /** * 丝杠导程 转/毫米 (r/mm) */ +#if MOTOR_REAL #define MOTOR_X_R_PER_MM 2.0 #define MOTOR_Y_R_PER_MM 5.0 #define MOTOR_Z_R_PER_MM 5.0 +#else +#define MOTOR_X_R_PER_MM 5.0 +#define MOTOR_Y_R_PER_MM 5.0 +#define MOTOR_Z_R_PER_MM 6.0 +#endif /** * 丝杠长度限制 毫米（mm） diff --git a/User/BSP/status/motor_manager.cpp b/User/BSP/status/motor_manager.cpp index cdfc030..d3ba6e7 100644 --- a/User/BSP/status/motor_manager.cpp +++ b/User/BSP/status/motor_manager.cpp @@ -54,7 +54,7 @@ uint8_t MotorManager::getMotorMovingStatus(int motorIndex) { if (motorIndex < 0 || motorIndex >= 3) { return 0; } - return motors[motorIndex].isMoving(); + return motors[motorIndex].isMoveFinished(); } // 获取某个电机的编码器位置 @@ -72,11 +72,3 @@ void MotorManager::setMotorEncoderPosition(int motorIndex, int32_t encoderPos) { } motors[motorIndex].setEncoderPosition(encoderPos); } - -// 新增设置电机是否运动的接口 -void MotorManager::setMotorMoving(int motorIndex, bool moving) { - if (motorIndex < 0 || motorIndex >= 3) { - return; - } - motors[motorIndex].setMoving(moving); -} diff --git a/User/BSP/status/motor_manager.h b/User/BSP/status/motor_manager.h index 3863063..d91db87 100644 --- a/User/BSP/status/motor_manager.h +++ b/User/BSP/status/motor_manager.h @@ -28,7 +28,6 @@ public: uint8_t getMotorMovingStatus(int motorIndex); int32_t getMotorEncoderPosition(int motorIndex); void setMotorEncoderPosition(int motorIndex, int32_t encoderPos); - void setMotorMoving(int motorIndex, bool moving); // 新增设置电机是否运动的接口 }; #endif //MOTOR_MANAGER_H diff --git a/User/BSP/tim_pwm/pwm_control.cpp b/User/BSP/tim_pwm/pwm_control.cpp index d1c38de..aa7d3d1 100644 --- a/User/BSP/tim_pwm/pwm_control.cpp +++ b/User/BSP/tim_pwm/pwm_control.cpp @@ -152,52 +152,7 @@ void TIM_PWM_Init(void) { // 更新指定通道的PWM频率和占空比 void Update_PWM(PWM_Channel_TypeDef channel, uint32_t frequency, uint8_t duty_cycle) { -#if 0 - if (frequency == 0) { - if (duty_cycle == 0) { - // 关闭定时器，GPIO输出0 - uint32_t tim_channel = Get_TIM_Channel(channel); - uint32_t gpio_pin = Get_GPIO_Pin(channel); - HAL_TIM_PWM_Stop(&htim, tim_channel); - HAL_GPIO_WritePin(GPIOx, gpio_pin, GPIO_PIN_RESET); - } else if (duty_cycle == 100) { - // GPIO输出1 - uint32_t tim_channel = Get_TIM_Channel(channel); - uint32_t gpio_pin = Get_GPIO_Pin(channel); - HAL_TIM_PWM_Stop(&htim, tim_channel); - HAL_GPIO_WritePin(GPIOx, gpio_pin, GPIO_PIN_SET); - } - } else { - // 计算定时器的周期和脉冲宽度 - uint32_t timer_clock = HAL_RCC_GetPCLK1Freq() * 2; // TIM4挂载在APB1总线上，APB1预分频器为2时，定时器时钟为APB1时钟的2倍 - uint32_t prescaler = (timer_clock / (frequency * 1000)) - 1; - uint32_t period = 1000; - uint32_t pulse = (period * duty_cycle) / 100; - - // 配置定时器参数 - htim.Instance->CR2 &= 0x00; - htim.Instance->PSC = prescaler; - htim.Instance->ARR = period; - - uint32_t tim_channel = Get_TIM_Channel(channel); - switch (tim_channel) { - case TIM_CHANNEL_1: - - htim.Instance->CCR1 = pulse; - HAL_TIM_PWM_Start(&htim, TIM_CHANNEL_1); - break; - case TIM_CHANNEL_2: - htim.Instance->CCR2 = pulse; - HAL_TIM_PWM_Start(&htim, TIM_CHANNEL_2); - break; - case TIM_CHANNEL_3: - htim.Instance->CCR3 = pulse; - HAL_TIM_PWM_Start(&htim, TIM_CHANNEL_3); - break; - } - htim.Instance->CR2 &= 0x00; - } -#else + // 计算定时器的周期和预分频器 uint32_t timer_clock = HAL_RCC_GetPCLK1Freq() * 2; // TIM4挂载在APB1总线上，APB1预分频器为2时，定时器时钟为APB1时钟的2倍 uint32_t prescaler = (timer_clock / (frequency * 1000)) - 1; @@ -221,11 +176,10 @@ void Update_PWM(PWM_Channel_TypeDef channel, uint32_t frequency, uint8_t duty_cy // htim.Instance->CCR1 = pulse; // htim.Instance->CCR2 = pulse; // htim.Instance->CCR3 = pulse; + __HAL_TIM_SET_PRESCALER(&htim, prescaler); __HAL_TIM_SET_AUTORELOAD(&htim, period); __HAL_TIM_SET_COMPARE(&htim, tim_channel, pulse); // 触发更新事件 HAL_TIM_GenerateEvent(&htim, TIM_EVENTSOURCE_UPDATE); - -#endif } \ No newline at end of file diff --git a/User/BSP/uart_cmd/cmd_process_service_process.cpp b/User/BSP/uart_cmd/cmd_process_service_process.cpp index cb6f16d..162efe5 100644 --- a/User/BSP/uart_cmd/cmd_process_service_process.cpp +++ b/User/BSP/uart_cmd/cmd_process_service_process.cpp @@ -21,24 +21,7 @@ using namespace iflytop; #define TAG "UART PARSE" -// int shortArrayToLittleEndianInt(short input[2]) { -// uint16_t littleEndianInput[2]; -// littleEndianInput[0] = SWAP_16((uint16_t)input[0]); -// littleEndianInput[1] = SWAP_16((uint16_t)input[1]); -// -// int result = 0; -// // 将第一个 short 左移 16 位，作为结果的高 16 位 -// result = (uint32_t) littleEndianInput[0] << 16; -// // 将第二个 short 作为结果的低 16 位 -// result += littleEndianInput[1] & 0xFFFF; -// return result; -// } -// -// void intToShortArray(int32_t value, uint16_t data[2]) { -// const uint32_t temp = (uint32_t)value; -// data[0] = (temp >> 16) & 0xFFFF; // 高 16 位（16-31 位） -// data[1] = temp & 0xFFFF; // 低 16 位（0-15 位） -// } +#define HG_ENABLE 0 // 使能高压包 extern int shortArrayToLittleEndianInt(short input[2]); extern void intToShortArray(int32_t value, uint16_t data[2]); @@ -67,10 +50,6 @@ std::vector handle_read_registers(uint16_t cmdId, uint16_t slaveId, uin void CmdProcessService_Process(uint8_t *msgPtr, size_t msg_len) { // 急停不接受任何指令 - if(AppHardware::ins()->isE_Stop()) { - return; - } - if (nullptr == msgPtr || msg_len < 6) { return; } @@ -83,6 +62,10 @@ void CmdProcessService_Process(uint8_t *msgPtr, size_t msg_len) { std::vector ack_msg; switch (func_code) { case CAN_PDU_WRITE_SINGLE_COIL: { + // 急停不接受控制指令 + if(AppHardware::ins()->isE_Stop()) { + return; + } if(msg_len < 8) { break; } @@ -95,6 +78,11 @@ void CmdProcessService_Process(uint8_t *msgPtr, size_t msg_len) { break; } case CAN_PDU_WRITE_SINGLE_REGISTER: { + // 急停不接受控制指令 + if(AppHardware::ins()->isE_Stop()) { + return; + } + if (msg_len < 8) { break; } @@ -105,6 +93,10 @@ void CmdProcessService_Process(uint8_t *msgPtr, size_t msg_len) { case CAN_PDU_WRITE_MULTIPLE_REGISTERS: { // 计算 PDU 长度：功能码（1 字节） + 起始地址（1 字节） + 寄存器数量（ // 计算 PDU 长度：功能码（1 字节） + 起始地址（1 字节） + 寄存器数量（1 字节） + 数据 + // 急停不接受控制指令 + if(AppHardware::ins()->isE_Stop()) { + return; + } if (msg_len < 11) { break; } else { @@ -264,6 +256,7 @@ std::vector handle_write_coil(uint16_t cmdId, uint16_t slaveId, uint16_ break; } case HIGH_VOLTAGE_PACK_POWER_ON_ADDRESS: { +#if HG_ENABLE if (value) { // 开启高压包供电 AppHardware::ins()->high_voltage_pack.turnOnPower(); @@ -274,6 +267,7 @@ std::vector handle_write_coil(uint16_t cmdId, uint16_t slaveId, uint16_ AppHardware::ins()->high_voltage_pack.turnOffPower();; ZLOGI(TAG, "SET HIGH VOLTAGE POWER OFF"); } +#endif break; } case PUMP_DRIVER_ADDRESS: { @@ -338,9 +332,11 @@ std::vector handle_write_regsiter(uint16_t cmdId, uint16_t slaveId, uin break; } case HIGH_VOLTAGE_PACK_SET_ADDRESS: { +#if HG_ENABLE const int32_t voltage = value; AppHardware::ins()->high_voltage_pack.turnOnWithVoltage(voltage); ZLOGI(TAG, "SET HIGH VOLTAGE PACK %d", voltage); +#endif break; } case LASER_POWER_ADDRESS: { @@ -433,6 +429,13 @@ std::vector handle_write_multi_regsiter(uint16_t cmdId, uint16_t slaveI case PUMP_DRIVER_SPEED_ADDRESS: { const int32_t i_flow = shortArrayToLittleEndianInt((short*)arr); const double d_flow = i_flow * 0.01; + AppHardware::ins()->pump_controller.moveWithFlowSpeed(d_flow); + ZLOGI(TAG, "SET MOVE PUMP SPEED ADDRESS %d", i_flow); + break;; + } + case PUMP_DRIVER_SET_SPEED_ADDRESS: { + const int32_t i_flow = shortArrayToLittleEndianInt((short*)arr); + const double d_flow = i_flow * 0.01; AppHardware::ins()->pump_controller.setFlowSpeed(d_flow); ZLOGI(TAG, "SET PUMP SPEED ADDRESS %d", i_flow); break;; @@ -483,11 +486,11 @@ std::vector handle_read_coils(uint16_t cmdId, uint16_t slaveId, uint16_ const bool isAtOrigin = true; const bool isAxisLimit = true; #else - const bool isMove = MotorManager::ins()->motors[motor_index].isMoving(); + const bool isMoveFinished = MotorManager::ins()->motors[motor_index].isMoveFinished(); const bool isAtOrigin = ExtiKeyManager::ins()->isPinTriggered(getORIGINPin(motor_index)); const bool isAxisLimit = ExtiKeyManager::ins()->isPinTriggered(getAxisLimitPin(motor_index)); #endif - data[0] = isMove ? 0xFF : 0x00; + data[0] = isMoveFinished ? 0xFF : 0x00; data[1] = isAtOrigin ? 0xFF : 0x00; data[2] = isAxisLimit ? 0xFF : 0x00; @@ -500,12 +503,12 @@ std::vector handle_read_coils(uint16_t cmdId, uint16_t slaveId, uint16_ break; } bool coils[count] = {0}; - coils[0] = !MotorManager::ins()->motors[0].isMoving(); - coils[1] = !MotorManager::ins()->motors[1].isMoving(); - coils[2] = !MotorManager::ins()->motors[2].isMoving(); - coils[3] = !MotorManager::ins()->motors[0].isHoming(); - coils[4] = !MotorManager::ins()->motors[1].isHoming(); - coils[5] = !MotorManager::ins()->motors[2].isHoming(); + coils[0] = MotorManager::ins()->motors[0].isMoveFinished(); + coils[1] = MotorManager::ins()->motors[1].isMoveFinished(); + coils[2] = MotorManager::ins()->motors[2].isMoveFinished(); + coils[3] = MotorManager::ins()->motors[0].isHomeSuc(); + coils[4] = MotorManager::ins()->motors[1].isHomeSuc(); + coils[5] = MotorManager::ins()->motors[2].isHomeSuc(); ack_msg = ModbusRtuProtocolFactory::createReadCoilsResponse(cmdId, slaveId, address, count, coils); break; } @@ -516,17 +519,17 @@ std::vector handle_read_coils(uint16_t cmdId, uint16_t slaveId, uint16_ } bool coils[count] = {0}; coils[0] = ExtiKeyManager::ins()->isPinTriggered(E_STOP_PIN); - coils[1] = ExtiKeyManager::ins()->isPinTriggered(SYSTEM_POWER_PIN); + coils[1] = AppHardware::ins()->isLaunched(); - coils[2] = !MotorManager::ins()->motors[0].isMoving(); + coils[2] = !MotorManager::ins()->motors[0].isMoveFinished(); coils[3] = ExtiKeyManager::ins()->isPinTriggered(getORIGINPin(0)); coils[4] = ExtiKeyManager::ins()->isPinTriggered(getAxisLimitPin(0)); - coils[5] = !MotorManager::ins()->motors[1].isMoving(); + coils[5] = !MotorManager::ins()->motors[1].isMoveFinished(); coils[6] = ExtiKeyManager::ins()->isPinTriggered(getORIGINPin(1)); coils[7] = ExtiKeyManager::ins()->isPinTriggered(getAxisLimitPin(1)); - coils[8] = !MotorManager::ins()->motors[2].isMoving(); + coils[8] = !MotorManager::ins()->motors[2].isMoveFinished(); coils[9] = ExtiKeyManager::ins()->isPinTriggered(getORIGINPin(2)); coils[10] = ExtiKeyManager::ins()->isPinTriggered(getAxisLimitPin(2)); @@ -543,7 +546,6 @@ std::vector handle_read_coils(uint16_t cmdId, uint16_t slaveId, uint16_ std::vector handle_read_registers(uint16_t cmdId, uint16_t slaveId, uint16_t address, uint16_t count) { std::vector ack_msg {}; - ZLOGI(TAG, "[READ REGISTERS] address %d count %d", (int)address, (int)(count)); uint16_t data[count] = {0}; switch (address) { case T_RH_READ_ADDRESS: { @@ -563,7 +565,6 @@ std::vector handle_read_registers(uint16_t cmdId, uint16_t slaveId, uin data[1] = temp; ack_msg = ModbusRtuProtocolFactory::createReadHoldingRegistersResponse(cmdId, slaveId, address, count, data); - ZLOGI(TAG, "READ T_RH_READ_ADDRESS"); break; } case FLOW_READ_ADDRESS: { @@ -631,15 +632,11 @@ std::vector handle_read_registers(uint16_t cmdId, uint16_t slaveId, uin break; } - for(int motor_index = 0; motor_index < 2; motor_index++) { - bool is_flip = ELCMotorHelper::isFlip(motor_index); - int32_t motor_pos = MotorManager::ins()->motors[motor_index].getEncoderPosition(); - motor_pos = is_flip ? -motor_pos : motor_pos; - + for(int motor_index = 0; motor_index < 3; motor_index++) { + const int32_t motor_pos = MotorManager::ins()->motors[motor_index].getEncoderPosition(); intToShortArray(motor_pos, data + 2 * motor_index); } ack_msg = ModbusRtuProtocolFactory::createReadHoldingRegistersResponse(cmdId, slaveId, address, count, data); - ZLOGI(TAG, "READ MOTOR [ALL] POS"); break; } case BOARD_INFO_READ_ADDRESS: { @@ -647,16 +644,11 @@ std::vector handle_read_registers(uint16_t cmdId, uint16_t slaveId, uin break; } - for(int motor_index = 0; motor_index < 2; motor_index++) { - bool is_flip = ELCMotorHelper::isFlip(motor_index); + for(int motor_index = 0; motor_index < 3; motor_index++) { int32_t motor_pos = MotorManager::ins()->motors[motor_index].getEncoderPosition(); - motor_pos = is_flip ? -motor_pos : motor_pos; - intToShortArray(motor_pos, data + 4 * motor_index); int32_t motor_speed = MotorManager::ins()->motors[motor_index].getRTSpeed(); - motor_speed = is_flip ? -motor_speed : motor_speed; - intToShortArray(motor_speed, data); intToShortArray(motor_speed, data + 4 * motor_index + 2); } @@ -669,7 +661,6 @@ std::vector handle_read_registers(uint16_t cmdId, uint16_t slaveId, uin data[15] = temp; ack_msg = ModbusRtuProtocolFactory::createReadHoldingRegistersResponse(cmdId, slaveId, address, count, data); - ZLOGI(TAG, "READ BOARD [ALL] STATUS"); break; } default: { diff --git a/User/BSP/uart_cmd/cmd_receive_service.c b/User/BSP/uart_cmd/cmd_receive_service.c index 96da974..488a397 100644 --- a/User/BSP/uart_cmd/cmd_receive_service.c +++ b/User/BSP/uart_cmd/cmd_receive_service.c @@ -15,26 +15,6 @@ static int32_t Local_CmdUartRxsize = 0; static bool Local_CmdUartRxbufIsReady = false; static void processCMDUartRxData(uint8_t* rx, size_t len) { - // if (!Protocol_IsHeaderRight(rx)) { - // ZLOGW(TAG, "CMDUartRxData Header Error %s", hex2str(rx, len)); - // return; - // } - // - // if (!Protocol_IsTailRight(rx)) { - // ZLOGW(TAG, "CMDUartRxData Tail Error %s", hex2str(rx, len)); - // return; - // } - // - // if (!Protocol_IsChecksumRight(rx)) { - // ZLOGW(TAG, "CMDUartRxData CheckSum Error %s", hex2str(rx, len)); - // return; - // } - // - // - // CmdProcessContext_t cxt; - // cxt.from = UART_CMD_CHANNEL; - // cxt.rx = rx; - // cxt.len = len; CmdProcessService_Process(rx, len); } diff --git a/User/app/app_core.cpp b/User/app/app_core.cpp index 3c1fbfd..52fd469 100644 --- a/User/app/app_core.cpp +++ b/User/app/app_core.cpp @@ -29,6 +29,18 @@ extern "C" { } #endif +/** + * 打印寄存器状态 + * @param reg + */ +void print_TMC5130DevStatusReg_t(TMC5130DevStatusReg_t reg); + +/** + * 打印寄存器状态 + * @param reg + */ +void print_TMC5130GState_t(TMC5130GState_t state); + void app_init() { ZLOGI(TAG, "======================= boardinfo ===================="); @@ -42,6 +54,7 @@ void app_init() AppHardware::ins()->SystemPowerOn(); #endif osDelay(100); + tri_color_light(COLOR_OFF); } void app_task() @@ -54,6 +67,12 @@ void app_task() int free_heap = xPortGetFreeHeapSize(); ZLOGI(TAG,"Free heap memory: %d bytes", free_heap); #endif +#if 0 + int enc_val_x = MotorManager::ins()->motors[0].getEncoderPosition(); + int enc_val_y = MotorManager::ins()->motors[1].getEncoderPosition(); + int enc_val_z = MotorManager::ins()->motors[2].getEncoderPosition(); + ZLOGI(TAG, "[x] %d [y]%d [z]%d", enc_val_x, enc_val_y, enc_val_z); +#endif } } @@ -167,3 +186,28 @@ void status_upload_timer_callback(void *argument) { ZLOGI("【TIMER】", "STATUS UPLAOD NORMAL"); } } + + +void print_TMC5130DevStatusReg_t(TMC5130DevStatusReg_t reg) { + printf("TMC5130DevStatusReg_t:\n"); + printf(" sg_result: %u\n", (unsigned int)reg.sg_result); + printf(" reserved0: %u\n", (unsigned int)reg.reserved0); + printf(" fsactive: %u\n", (unsigned int)reg.fsactive); + printf(" cs_actual: %u\n", (unsigned int)reg.cs_actual); + printf(" reserved1: %u\n", (unsigned int)reg.reserved1); + printf(" stallguard: %u\n", (unsigned int)reg.stallguard); + printf(" ot: %u\n", (unsigned int)reg.ot); + printf(" otpw: %u\n", (unsigned int)reg.otpw); + printf(" s2ga: %u\n", (unsigned int)reg.s2ga); + printf(" s2gb: %u\n", (unsigned int)reg.s2gb); + printf(" ola: %u\n", (unsigned int)reg.ola); + printf(" olb: %u\n", (unsigned int)reg.olb); + printf(" stst: %u\n", (unsigned int)reg.stst); +} + +void print_TMC5130GState_t(TMC5130GState_t state) { + printf("TMC5130GState_t:\n"); + printf(" reset: %u\n", (unsigned int)state.reset); + printf(" drv_err: %u\n", (unsigned int)state.drv_err); + printf(" uv_cp: %u\n", (unsigned int)state.uv_cp); +}