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

基质喷涂V2 修复

develop
HSZ_HeSongZhen 2 weeks ago
parent
2ba2d420c7
commit
3e7f7d8545
5 changed files with 63 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      User/BSP/base/apphardware.cpp
  2. 64
      User/BSP/status/elc_motor.cpp
  3. 12
      User/BSP/status/elc_motor.h
  4. 17
      User/BSP/status/elc_motor_helper.cpp
  5. 6
      User/components/tmcdriver/tmc51x0/tmc51x0.cpp

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

@ -55,7 +55,7 @@ void AppHardware::initialize() {
#if 0 #if 0
MOTO1.setIHOLD_IRUN(0, 7, 100); MOTO1.setIHOLD_IRUN(0, 7, 100);
#else #else
MOTO1.setIHOLD_IRUN(0, 8, 100);
MOTO1.setIHOLD_IRUN(5, 10, 100);
#endif #endif
MOTO1.setScale(1000); MOTO1.setScale(1000);
MOTO1.setEncResolution(-1000); MOTO1.setEncResolution(-1000);
@ -68,7 +68,7 @@ void AppHardware::initialize() {
#if 0 #if 0
MOTO2.setIHOLD_IRUN(0, 10, 100); MOTO2.setIHOLD_IRUN(0, 10, 100);
#else #else
MOTO2.setIHOLD_IRUN(0, 8, 100);
MOTO2.setIHOLD_IRUN(5, 10, 100);
#endif #endif
MOTO2.setScale(1000); MOTO2.setScale(1000);
MOTO2.setEncResolution(-1000); MOTO2.setEncResolution(-1000);
@ -78,7 +78,7 @@ void AppHardware::initialize() {
MOTO3.initialize(2, tmc5130cfg3); MOTO3.initialize(2, tmc5130cfg3);
MOTO3.enable(false); MOTO3.enable(false);
MOTO3.setIHOLD_IRUN(0, 8, 100);
MOTO3.setIHOLD_IRUN(5, 10, 100);
MOTO3.setScale(1000); MOTO3.setScale(1000);
MOTO3.setEncResolution(-1000); MOTO3.setEncResolution(-1000);

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

@ -65,7 +65,7 @@ void ECLMotor::readMotorPosition() {
if (!last_is_move_finished && is_move_finished) { if (!last_is_move_finished && is_move_finished) {
//处理移动结束的问题 //处理移动结束的问题
if (!isHomeSuc()) {
if (isRunHomingTask) {
ZLOGI(TAG, "[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED"); ZLOGI(TAG, "[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED");
osMutexAcquire(mutex_, osWaitForever); osMutexAcquire(mutex_, osWaitForever);
home_event_.event_type = EVENT_MOTOR_MOVE_FINISHED; home_event_.event_type = EVENT_MOTOR_MOVE_FINISHED;
@ -106,7 +106,7 @@ ECLMotor::ECLMotor() {
is_home_init_ = false; is_home_init_ = false;
is_home_suc_ = false; is_home_suc_ = false;
home_state_ = STATE_INIT; home_state_ = STATE_INIT;
isRunHomingTask = true;
isRunHomingTask = false;
// 创建定时器 // 创建定时器
osTimerAttr_t timerAttr = { osTimerAttr_t timerAttr = {
@ -263,10 +263,6 @@ void ECLMotor::setMotorIndex(int32_t index) {
this->index_ = index; this->index_ = index;
} }
bool ECLMotor::isHomeInit() {
return is_home_init_;
}
bool ECLMotor::isHomeSuc() { bool ECLMotor::isHomeSuc() {
osMutexAcquire(mutex_, osWaitForever); osMutexAcquire(mutex_, osWaitForever);
bool is_home_suc = is_home_suc_; bool is_home_suc = is_home_suc_;
@ -288,6 +284,7 @@ void ECLMotor::runhomeSuc() {
this->is_home_init_ = true; this->is_home_init_ = true;
this->is_home_suc_ = true; this->is_home_suc_ = true;
this->home_state_ = STATE_INIT; this->home_state_ = STATE_INIT;
isRunHomingTask = false;
osMutexRelease(mutex_); osMutexRelease(mutex_);
ZLOGI(TAG, "【 MOVE TO HOME SUC 】"); ZLOGI(TAG, "【 MOVE TO HOME SUC 】");
@ -300,23 +297,26 @@ void ECLMotor::runhomeSuc() {
} }
void ECLMotor::startMoveHome() { void ECLMotor::startMoveHome() {
home_start_tick_ = 0;
osMutexAcquire(mutex_, osWaitForever); osMutexAcquire(mutex_, osWaitForever);
this->is_home_suc_ = false; this->is_home_suc_ = false;
isRunHomingTask = true;
osMutexRelease(mutex_); osMutexRelease(mutex_);
const bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_); const bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_);
// 当前是否在原点 // 当前是否在原点
if (isAtOrigin) { if (isAtOrigin) {
osMutexAcquire(mutex_, osWaitForever);
home_event_.event_type = EVENT_IN_ORIGIN; home_event_.event_type = EVENT_IN_ORIGIN;
osMutexRelease(mutex_);
ZLOGI(TAG, "[PUT] EVENT IN ORIGIN"); ZLOGI(TAG, "[PUT] EVENT IN ORIGIN");
osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
if (status != osOK) { if (status != osOK) {
ZLOGE(TAG, "EVENT IN ORIGIN SEND FAIL, status: %d", status); ZLOGE(TAG, "EVENT IN ORIGIN SEND FAIL, status: %d", status);
} }
} else { } else {
osMutexAcquire(mutex_, osWaitForever);
home_event_.event_type = EVENT_START_HOMING; home_event_.event_type = EVENT_START_HOMING;
osMutexRelease(mutex_);
ZLOGI(TAG, "[PUT] EVENT_START_HOMING"); ZLOGI(TAG, "[PUT] EVENT_START_HOMING");
osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
if (status != osOK) { if (status != osOK) {
@ -337,8 +337,11 @@ void ECLMotor::homingTask(void *arg) {
ECLMotor *eclMotor = static_cast<ECLMotor *>(arg); ECLMotor *eclMotor = static_cast<ECLMotor *>(arg);
HomeEvent home_event; HomeEvent home_event;
while (eclMotor->isRunHomingTask) {
while (true) {
osStatus status = osMessageQueueGet(eclMotor->homeEventQueue, &home_event, NULL, osWaitForever); osStatus status = osMessageQueueGet(eclMotor->homeEventQueue, &home_event, NULL, osWaitForever);
if(!eclMotor->isRunHomingTask) {
continue;
}
if (status == osOK) { if (status == osOK) {
TMC51X0 *motor = AppHardware::ins()->getPump(eclMotor->index_); TMC51X0 *motor = AppHardware::ins()->getPump(eclMotor->index_);
if (motor == nullptr) { if (motor == nullptr) {
@ -354,7 +357,7 @@ void ECLMotor::homingTask(void *arg) {
int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
int absSpeed = abs(speed); int absSpeed = abs(speed);
eclMotor->ECL_Rotate(-absSpeed); eclMotor->ECL_Rotate(-absSpeed);
ZLOGI(TAG, "【HOME】 BACK TO ORIGIN MIDDLE");
ZLOGI(TAG, "【HOME】 BACK TO ORIGIN MIDDLE 中速回原点");
break; break;
} }
case EVENT_ORIGIN_ENTER: { case EVENT_ORIGIN_ENTER: {
@ -373,7 +376,7 @@ void ECLMotor::homingTask(void *arg) {
const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed); eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed);
// 4000 4 圈 // 4000 4 圈
ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] FORWARD TO ORIGIN MIDDLE");
ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] FORWARD TO ORIGIN MIDDLE 中速向前10mm");
} }
// if second enter // if second enter
if (eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_LOW) { if (eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_LOW) {
@ -381,11 +384,12 @@ void ECLMotor::homingTask(void *arg) {
// 电机停止 // 电机停止
// 回归原点成功 // 回归原点成功
eclMotor->runhomeSuc(); eclMotor->runhomeSuc();
ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] HOME SUC 回原点成功");
} }
break; break;
} }
case EVENT_IN_ORIGIN: { case EVENT_IN_ORIGIN: {
ZLOGI(TAG, "[]EVENT IN ORIGIN");
ZLOGI(TAG, "[]EVENT IN ORIGIN 进入原点");
// 中速向前10mm // 中速向前10mm
motor->setEncVal(0); motor->setEncVal(0);
motor->setXACTUAL(0); motor->setXACTUAL(0);
@ -397,28 +401,40 @@ void ECLMotor::homingTask(void *arg) {
const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed); eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed);
ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE");
ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE 中速向前10mm");
break; break;
} }
case EVENT_MOTOR_MOVE_FINISHED: { case EVENT_MOTOR_MOVE_FINISHED: {
ZLOGI(TAG, "[]EVENT MOTOR MOVE FINISHED");
if (eclMotor->home_state_ == STATE_LEAVE_ORIGIN_MIDDLE) {
ZLOGI(TAG, "[]EVENT MOTOR MOVE FINISHED 移动结束");
osMutexAcquire(eclMotor->mutex_, osWaitForever);
HomeState home_state =eclMotor->home_state_;
bool is_move_finished = eclMotor->is_move_finished_;
osMutexRelease(eclMotor->mutex_);
if(!is_move_finished) {
// 电机没有移动结束
ZLOGI(TAG, "【HOME】 MOTOR NOT MOVE FINISHED 电机没有移动结束");
break;
}
if (home_state == STATE_LEAVE_ORIGIN_MIDDLE) {
// 低速回归 // 低速回归
osMutexAcquire(eclMotor->mutex_, osWaitForever); osMutexAcquire(eclMotor->mutex_, osWaitForever);
eclMotor->home_state_ = STATE_BACK_TO_ORIGIN_LOW; eclMotor->home_state_ = STATE_BACK_TO_ORIGIN_LOW;
osMutexRelease(eclMotor->mutex_); osMutexRelease(eclMotor->mutex_);
const int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_); const int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_);
eclMotor->ECL_Rotate(-speed); eclMotor->ECL_Rotate(-speed);
ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW");
ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW 低速回原点");
} }
break; break;
} }
default: default:
ZLOGI(TAG, "【HOME】 未处理事件类型 EVENT TYPE %d", home_event.event_type);
break; break;
} }
#endif #endif
} else { } else {
ZLOGI(TAG, "GET MESSAGE FAILED");
} }
} }
} }
@ -430,7 +446,7 @@ void ECLMotor::moveToHome() {
void ECLMotor::runZeroLimit(bool isEnter) { void ECLMotor::runZeroLimit(bool isEnter) {
// 处理回home 位逻辑 // 处理回home 位逻辑
if (!isHomeSuc()) {
if (isRunHomingTask) {
TMC51X0 *motor = AppHardware::ins()->getPump(index_); TMC51X0 *motor = AppHardware::ins()->getPump(index_);
if (motor == nullptr) { if (motor == nullptr) {
return; return;
@ -439,11 +455,8 @@ void ECLMotor::runZeroLimit(bool isEnter) {
if (isEnter) { if (isEnter) {
home_event_.event_type = EVENT_ORIGIN_ENTER; home_event_.event_type = EVENT_ORIGIN_ENTER;
motor->stop(); motor->stop();
osDelay(10);
ZLOGI(TAG, "[PUT] EVENT_ORIGIN_ENTER");
} else { } else {
home_event_.event_type = EVENT_ORIGIN_LEAVE; home_event_.event_type = EVENT_ORIGIN_LEAVE;
ZLOGI(TAG, "[PUT] EVENT_ORIGIN_LEAVE");
} }
osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
@ -486,10 +499,6 @@ void ECLMotor::runEndLimit(bool isEnter) {
void ECLMotor::runE_Stop() { void ECLMotor::runE_Stop() {
// 发送CAN 消息 // 发送CAN 消息
osMutexAcquire(mutex_, osWaitForever);
this->is_home_suc_ = false;
osMutexRelease(mutex_);
this->runStop(); this->runStop();
} }
@ -499,11 +508,16 @@ void ECLMotor::runPause() {
} }
void ECLMotor::runStop() { void ECLMotor::runStop() {
osMutexAcquire(mutex_, osWaitForever);
this->isRunHomingTask = false;
osMutexRelease(mutex_);
TMC51X0 *motor = AppHardware::ins()->getPump(index_); TMC51X0 *motor = AppHardware::ins()->getPump(index_);
if (motor == nullptr) { if (motor == nullptr) {
return; return;
} }
motor->stop(); motor->stop();
// 发送CAN 消息 // 发送CAN 消息
#if CAN_MODULE_ENABLE #if CAN_MODULE_ENABLE
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Move_Finished, index_); CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Move_Finished, index_);

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

@ -59,7 +59,6 @@ private:
int32_t target_pos_; // 目标位置 int32_t target_pos_; // 目标位置
int32_t target_speed_; // 目标速度 int32_t target_speed_; // 目标速度
bool is_move_finished_; // 是否已经移动结束 bool is_move_finished_; // 是否已经移动结束
bool isFoward_; // 是否正向运动
osTimerId_t readTimer; osTimerId_t readTimer;
osMutexId_t mutex_; osMutexId_t mutex_;
@ -70,10 +69,7 @@ private:
HomeEvent home_event_; HomeEvent home_event_;
osMessageQueueId_t homeEventQueue; // 回原点事件队列 osMessageQueueId_t homeEventQueue; // 回原点事件队列
HomeState home_state_{STATE_INIT}; // home 状态机 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_init_{ false }; // 是否已经第一次（初始化回过原点）回过原点
bool is_home_suc_ { false }; // 是否已经成功回原点 bool is_home_suc_ { false }; // 是否已经成功回原点
@ -87,14 +83,9 @@ public:
~ECLMotor(); ~ECLMotor();
void setMotorIndex(int32_t index); void setMotorIndex(int32_t index);
// 是否已经回归到原点
bool isHomeInit();
// 是否正在回原点的逻辑 // 是否正在回原点的逻辑
bool isHomeSuc(); bool isHomeSuc();
void runhomeSuc();
void startMoveHome(); void startMoveHome();
void setHomeSeqId(uint8_t seq_id); void setHomeSeqId(uint8_t seq_id);
@ -128,7 +119,8 @@ public:
void setEncoderPosition(int32_t encoderPos); void setEncoderPosition(int32_t encoderPos);
int32_t getRTSpeed(); int32_t getRTSpeed();
private:
void runhomeSuc();
// 回原点任务 // 回原点任务
static void homingTask(void *arg); static void homingTask(void *arg);

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

@ -14,6 +14,8 @@ const int32_t ELCMotorHelper::rEncPosLimitArray[MOTOR_NUM] = {
}; };
#if MOTOR_REAL #if MOTOR_REAL
#if SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V1
const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = {
600, 600, 600 600, 600, 600
}; };
@ -23,6 +25,18 @@ const int32_t ELCMotorHelper::rPerMinMiddleSpeedArray[MOTOR_NUM] = {
const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = { const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = {
60, 30, 30 60, 30, 30
}; };
#elif SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V2
const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = {
600, 600, 600
};
const int32_t ELCMotorHelper::rPerMinMiddleSpeedArray[MOTOR_NUM] = {
300, 300, 300
};
const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = {
60, 60, 60
};
#else
#endif // SUB_SPRAY_VERVION
#else #else
const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = {
600, 600, 600 600, 600, 600
@ -42,8 +56,9 @@ const int32_t ELCMotorHelper::encLockRotorTH[MOTOR_NUM] = {500, 500, 500}; //
const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, false, true}; const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, false, true};
#elif SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V2 #elif SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V2
const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, true, false}; const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, true, false};
// const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, false, false};
#else #else
#endif #endif

6
User/components/tmcdriver/tmc51x0/tmc51x0.cpp
View File

@ -87,16 +87,16 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) {
writeInt(TMC5130_VACTUAL, 0x00000000); writeInt(TMC5130_VACTUAL, 0x00000000);
writeInt(TMC5130_TZEROWAIT, 0); writeInt(TMC5130_TZEROWAIT, 0);
setVstart(100);
setVstart(0);
setA1(15); setA1(15);
setAmax(15); setAmax(15);
setV1(500); setV1(500);
setDmax(15); setDmax(15);
setD1(15); setD1(15);
setVstop(100);
setVstop(10);
setTzerowait(100); setTzerowait(100);
setIHOLD_IRUN(0, 30, 100);
setIHOLD_IRUN(0, 5, 100);
enable(false); enable(false);
} }

Write Preview
Loading…
Cancel
Save