HSZ_HeSongZhen
/
Sub_Spray_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

1. 优化回原点功能 

2. 增加三色灯控制
develop
HSZ_HeSongZhen 4 months ago
parent
0499feacfc
commit
59ff433ed9
20 changed files with 315 additions and 477 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      Core/Src/freertos.c
  2. 6
      User/BSP/LED/exti_key_manager.cpp
  3. 23
      User/BSP/LED/led.c
  4. 27
      User/BSP/LED/led.h
  5. 23
      User/BSP/LED/pump_controller.cpp
  6. 2
      User/BSP/LED/pump_controller.h
  7. 46
      User/BSP/base/apphardware.cpp
  8. 2
      User/BSP/base/apphardware.hpp
  9. 1
      User/BSP/can_control/can_config.h
  10. 4
      User/BSP/can_control/can_protocol_parser.cpp
  11. 344
      User/BSP/status/elc_motor.cpp
  12. 36
      User/BSP/status/elc_motor.h
  13. 12
      User/BSP/status/elc_motor_helper.cpp
  14. 9
      User/BSP/status/elc_motor_helper.h
  15. 10
      User/BSP/status/motor_manager.cpp
  16. 1
      User/BSP/status/motor_manager.h
  17. 48
      User/BSP/tim_pwm/pwm_control.cpp
  18. 89
      User/BSP/uart_cmd/cmd_process_service_process.cpp
  19. 20
      User/BSP/uart_cmd/cmd_receive_service.c
  20. 44
      User/app/app_core.cpp

3
Core/Src/freertos.c
View File

@ -170,9 +170,6 @@ void feedDog(void)
void StartIDLETask(void *argument) {
for(;;)
{
// LED_GREEN_TOGGLE;
// LED_RED_TOGGLE;
// LED_BLUE_TOGGLE;
feedDog();
osDelay(2000);
}

6
User/BSP/LED/exti_key_manager.cpp
View File

@ -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;
}

23
User/BSP/LED/led.c
View File

@ -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;
}
}

27
User/BSP/LED/led.h
View File

@ -16,25 +16,25 @@ 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
// 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_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
// 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_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
// 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_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
@ -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

23
User/BSP/LED/pump_controller.cpp
View File

@ -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();

2
User/BSP/LED/pump_controller.h
View File

@ -45,6 +45,8 @@ public:
// 设置注射泵流速
void setFlowSpeed(double flow);
void moveWithFlowSpeed(double flow);
// 设备是否正常
bool isNoraml();

46
User/BSP/base/apphardware.cpp
View File

@ -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() {
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;
}

2
User/BSP/base/apphardware.hpp
View File

@ -97,7 +97,7 @@ class AppHardware {
void SystemPowerOn();
void SystemPowerOff();
void SystemPowerOff(bool is_force = false);
void setTJCScreenInDownloadMode();

1
User/BSP/can_control/can_config.h
View File

@ -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

4
User/BSP/can_control/can_protocol_parser.cpp
View File

@ -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

344
User/BSP/status/elc_motor.cpp
View File

@ -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,42 +298,37 @@ 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");
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);
}
}
}
}
void ECLMotor::setHomeSeqId(uint8_t seq_id) {
this->seq_id_home_ = 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) {
ZLOGI(TAG, "[]EVENT IN ORIGIN");
// 中速向前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);
// 前向移动 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
}

36
User/BSP/status/elc_motor.h
View File

@ -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);

12
User/BSP/status/elc_motor_helper.cpp
View File

@ -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}; // 电机差半圈

9
User/BSP/status/elc_motor_helper.h
View File

@ -6,17 +6,26 @@
#define ELC_MOTOR_HELPER_H
#include <cstdint>
#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）

10
User/BSP/status/motor_manager.cpp
View File

@ -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);
}

1
User/BSP/status/motor_manager.h
View File

@ -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

48
User/BSP/tim_pwm/pwm_control.cpp
View File

@ -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
}

89
User/BSP/uart_cmd/cmd_process_service_process.cpp
View File

@ -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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> handle_read_coils(uint16_t cmdId, uint16_t slaveId, uint16_
std::vector<uint8_t> handle_read_registers(uint16_t cmdId, uint16_t slaveId, uint16_t address, uint16_t count) {
std::vector<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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: {

20
User/BSP/uart_cmd/cmd_receive_service.c
View File

@ -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);
}

44
User/app/app_core.cpp
View File

@ -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);
}
Write Preview
Loading…
Cancel
Save