You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
710 lines
20 KiB
710 lines
20 KiB
//
|
|
// Created by iflyt on 2025/3/3.
|
|
//
|
|
|
|
#include "elc_motor.h"
|
|
|
|
#include <base/apphal.hpp>
|
|
#include <base/apphardware.hpp>
|
|
#include <can_control/can_protocol_factory.h>
|
|
#include <cmath>
|
|
|
|
#include "elc_motor_helper.h"
|
|
|
|
#define TEST 1
|
|
|
|
#define TAG "ECL MOTOR"
|
|
|
|
#define TIMER_IS_FINE 0
|
|
|
|
using namespace iflytop;
|
|
|
|
|
|
// 定时器回调函数
|
|
void ECLMotor::readTimerCallback(void *arg) {
|
|
ECLMotor *motor = static_cast<ECLMotor *>(arg);
|
|
motor->readMotorPosition();
|
|
}
|
|
|
|
// 读取电机位置
|
|
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;
|
|
|
|
|
|
int32_t x_actual = 0;
|
|
int32_t enc_val = 0;
|
|
int32_t rt_speed = 0;
|
|
|
|
enc_val = motor->getEncVal(); // 获取编码器值
|
|
x_actual = motor->getXACTUAL(); // 获取实时位置
|
|
rt_speed = motor->getVACTUAL(); // 获取实时速度
|
|
|
|
TMC5130RampStat ramp_stat = motor->getRampStat();
|
|
is_reached = 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;
|
|
}
|
|
x_actual_ = x_actual;
|
|
enc_val_ = enc_val;
|
|
rt_speed_ = rt_speed;
|
|
osMutexRelease(mutex_);
|
|
|
|
if (is_moving && is_move_finished) {
|
|
//处理移动结束的问题
|
|
#if 1
|
|
if (isHoming()) {
|
|
ZLOGI(TAG, "【[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED】");
|
|
#if 1
|
|
home_event_.event_type = EVENT_MOTOR_MOVE_FINISHED;
|
|
osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
|
|
#endif
|
|
}
|
|
#endif
|
|
// 发送CAN 消息
|
|
{
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_finished_, Motor_Move_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
}
|
|
ZLOGI(TAG, "【FIRST DETECT】 MOVE FINISHED");
|
|
}
|
|
}
|
|
|
|
// 设置读取速率
|
|
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);
|
|
}
|
|
|
|
ECLMotor::ECLMotor() {
|
|
// 创建互斥锁
|
|
mutex_ = osMutexNew(NULL);
|
|
if (mutex_ == NULL) {
|
|
// 处理互斥锁创建失败
|
|
ZLOGI(TAG, "mutex_ create failed");
|
|
}
|
|
|
|
// 初始化电机位置和目标位置
|
|
x_actual_ = 0;
|
|
target_pos_ = 0;
|
|
target_speed_ = 10; // 10 / 60 * 6s ==== 6 S 可以转一圈
|
|
is_moving_ = 0;
|
|
enc_val_ = 0;
|
|
|
|
isHomed_ = false;
|
|
homeState_ = STATE_INIT;
|
|
isRunHomingTask = true;
|
|
|
|
// 初始读取速率
|
|
readRate = READ_RATE_HIGH;
|
|
|
|
// 创建定时器
|
|
osTimerAttr_t timerAttr = {
|
|
.name = "MotorReadTimer",
|
|
.attr_bits = 0U,
|
|
.cb_mem = NULL,
|
|
.cb_size = 0U
|
|
};
|
|
readTimer = osTimerNew(readTimerCallback, osTimerPeriodic, this, &timerAttr);
|
|
if (readTimer == NULL) {
|
|
// 处理定时器创建失败
|
|
}
|
|
|
|
// 创建消息队列
|
|
homeEventQueue = osMessageQueueNew(10, sizeof(HomeEvent), NULL);
|
|
if (homeEventQueue == NULL) {
|
|
// 处理消息队列创建失败的情况
|
|
ZLOGE(TAG, "Failed to create home event message queue");
|
|
}
|
|
|
|
// 创建任务并获取线程句柄
|
|
home_task_attr.name = "HomeStateTask";
|
|
home_task_attr.stack_size = 256 * 8;
|
|
home_task_attr.priority = osPriorityHigh;
|
|
homingStateMachineThreadId = osThreadNew(homingTask, this, &home_task_attr);
|
|
if (homingStateMachineThreadId == NULL) {
|
|
// 处理线程创建失败的情况
|
|
ZLOGE(TAG, "THREAD INDEX %d Create Failed", index_);
|
|
}
|
|
|
|
|
|
// 启动定时器
|
|
setReadRate(readRate);
|
|
}
|
|
|
|
ECLMotor::~ECLMotor() {
|
|
isRunHomingTask = false;
|
|
if (homingStateMachineThreadId != NULL) {
|
|
osThreadTerminate(homingStateMachineThreadId);
|
|
}
|
|
if (homeEventQueue != NULL) {
|
|
osMessageQueueDelete(homeEventQueue);
|
|
}
|
|
osMutexDelete(mutex_);
|
|
osTimerDelete(readTimer);
|
|
}
|
|
|
|
// 读取电机的位置
|
|
int32_t ECLMotor::getPosition() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
int32_t pos = x_actual_;
|
|
osMutexRelease(mutex_);
|
|
return pos;
|
|
}
|
|
|
|
// 设置电机的目标位置
|
|
void ECLMotor::setTargetPosition(int32_t target) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
|
|
if (ELCMotorHelper::isFlip(index_)) {
|
|
target = -target;
|
|
ZLOGI(TAG, "【MOTOR 【%d】TARGET FLIP】", index_);
|
|
}
|
|
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) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
if (ELCMotorHelper::isFlip(index_)) {
|
|
shift_target = -shift_target;
|
|
ZLOGI(TAG, "【MOTOR 【%d】SHIFT TARGET FLIP】", index_);
|
|
}
|
|
motor->moveBy(shift_target, abs(target_speed_));
|
|
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
target_pos_ = target_pos_ + shift_target;
|
|
is_moving_ = true;
|
|
osMutexRelease(mutex_);
|
|
}
|
|
|
|
void ECLMotor::moveToWithSpeed(int32_t target, int32_t speed) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
if (ELCMotorHelper::isFlip(index_)) {
|
|
target = -target;
|
|
ZLOGI(TAG, "【MOTOR 【%d】TARGET FLIP】", index_);
|
|
}
|
|
motor->moveTo(target, abs(speed));
|
|
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
target_pos_ = target;
|
|
is_moving_ = true;
|
|
osMutexRelease(mutex_);
|
|
}
|
|
|
|
|
|
// 设置电机速度
|
|
void ECLMotor::setSpeed(int32_t speed) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
this->target_speed_ = speed;
|
|
#if 0
|
|
if(is_moving_) {
|
|
motor->moveTo(target_pos_, this->target_speed_);
|
|
}
|
|
#else
|
|
if (is_moving_) {
|
|
motor->setVMax(this->target_speed_);
|
|
}
|
|
#endif
|
|
osMutexRelease(mutex_);
|
|
}
|
|
|
|
// 获取电机速度
|
|
int32_t ECLMotor::getRTSpeed() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
const int32_t spd = rt_speed_;
|
|
osMutexRelease(mutex_);
|
|
return spd;
|
|
}
|
|
|
|
// 获取电机运动状态
|
|
bool ECLMotor::isMoving() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
bool moving = is_moving_;
|
|
osMutexRelease(mutex_);
|
|
return moving;
|
|
}
|
|
|
|
// 获取编码器位置
|
|
int32_t ECLMotor::getEncoderPosition() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
int32_t encPos = enc_val_;
|
|
osMutexRelease(mutex_);
|
|
return encPos;
|
|
}
|
|
|
|
// 设置编码器位置
|
|
void ECLMotor::setEncoderPosition(int32_t encoderPos) {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
enc_val_ = 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::isHoming() {
|
|
bool isDoHome = homeState_ != STATE_INIT;
|
|
return isDoHome;
|
|
}
|
|
|
|
void ECLMotor::runhomeSuc() {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
// 发送 CAN 信息
|
|
motor->setEncVal(0);
|
|
motor->setXACTUAL(0);
|
|
this->isHomed_ = true;
|
|
this->homeState_ = STATE_INIT;
|
|
|
|
ZLOGI(TAG, "【HOMED】 MOVE TO HOME SUC"); {
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Home_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
}
|
|
}
|
|
|
|
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")
|
|
// 当前是否在原点
|
|
if (isAtOrigin) {
|
|
home_event_.event_type = EVENT_IN_ORIGIN;
|
|
ZLOGI(TAG, "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);
|
|
}
|
|
} 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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void ECLMotor::homingTask(void *arg) {
|
|
ECLMotor *eclMotor = static_cast<ECLMotor *>(arg);
|
|
HomeEvent home_event;
|
|
|
|
while (eclMotor->isRunHomingTask) {
|
|
osStatus status = osMessageQueueGet(eclMotor->homeEventQueue, &home_event, NULL, osWaitForever);
|
|
if (status == osOK) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(eclMotor->index_);
|
|
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;
|
|
// 中速倒转
|
|
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) {
|
|
// 停止
|
|
// 中速向前10mm
|
|
// int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
|
|
// eclMotor->ECL_Rotate(speed);
|
|
eclMotor->homeState_ = STATE_LEAVE_ORIGIN_MIDDLE;
|
|
|
|
// 位置清零
|
|
motor->setEncVal(0);
|
|
motor->setXACTUAL(0);
|
|
|
|
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) {
|
|
// 位置清零
|
|
// 电机停止
|
|
// 回归原点成功
|
|
eclMotor->runhomeSuc();
|
|
}
|
|
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");
|
|
}
|
|
break;
|
|
}
|
|
case EVENT_MOTOR_MOVE_FINISHED: {
|
|
ZLOGI(TAG, "[]EVENT_MOTOR_MOVE_FINISHED");
|
|
if (eclMotor->homeState_ == STATE_LEAVE_ORIGIN_MIDDLE) {
|
|
// 低速回归
|
|
eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_LOW;
|
|
const int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_);
|
|
eclMotor->ECL_Rotate(-speed);
|
|
ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW");
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
#endif
|
|
} 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()) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
|
|
if (isEnter) {
|
|
home_event_.event_type = EVENT_ORIGIN_ENTER;
|
|
motor->stop();
|
|
osDelay(10);
|
|
ZLOGI(TAG, "[PUT] EVENT_ORIGIN_ENTER");
|
|
} else {
|
|
home_event_.event_type = EVENT_ORIGIN_LEAVE;
|
|
ZLOGI(TAG, "[PUT] EVENT_ORIGIN_LEAVE");
|
|
}
|
|
|
|
osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
|
|
|
|
return;
|
|
}
|
|
|
|
if (!isEnter) {
|
|
return;
|
|
}
|
|
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
motor->stop();
|
|
osDelay(10); {
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_finished_, Motor_Move_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
}
|
|
osDelay(10);
|
|
// 重置编码器 和 位置
|
|
motor->setXACTUAL(0);
|
|
motor->setEncVal(0);
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
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_);
|
|
}
|
|
|
|
void ECLMotor::runEndLimit(bool isEnter) {
|
|
// 发送CAN 消息
|
|
if (!isEnter) {
|
|
return;
|
|
}
|
|
this->runStop();
|
|
}
|
|
|
|
void ECLMotor::runE_Stop() {
|
|
// 发送CAN 消息
|
|
this->runStop();
|
|
}
|
|
|
|
void ECLMotor::runPause() {
|
|
// 发送CAN 消息
|
|
this->runStop();
|
|
}
|
|
|
|
void ECLMotor::runStop() {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
motor->stop();
|
|
// 发送CAN 消息
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Move_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
|
|
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;
|
|
}
|
|
|
|
if (ELCMotorHelper::isFlip(index_) && is_flip) {
|
|
speed = -speed;
|
|
ZLOGI(TAG, "ECL MOTOR ECL REAL INDEX 【%d】 FLIP", index_);
|
|
}
|
|
|
|
motor->rotate(speed);
|
|
ZLOGI(TAG, "ECL MOTOR %d SPEED %d", index_, speed);
|
|
#else
|
|
motor->rotate(roback_speed);
|
|
#endif
|
|
}
|