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.
541 lines
16 KiB
541 lines
16 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() {
|
|
// 清除标志位
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
bool last_is_move_finished = is_move_finished_;
|
|
osMutexRelease(mutex_);
|
|
|
|
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();
|
|
bool is_move_finished = motor->isReachTarget(&ramp_stat);
|
|
|
|
int32_t postion = 0;
|
|
if (0 == enc_val) {
|
|
postion = x_actual;
|
|
} else {
|
|
postion = enc_val;
|
|
}
|
|
// 移动中
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
is_move_finished_ = is_move_finished;
|
|
x_actual_ = x_actual;
|
|
enc_val_ = enc_val;
|
|
rt_speed_ = rt_speed;
|
|
osMutexRelease(mutex_);
|
|
|
|
if (!last_is_move_finished && is_move_finished) {
|
|
//处理移动结束的问题
|
|
if (isRunHomingTask) {
|
|
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);
|
|
}
|
|
// 发送CAN 消息
|
|
{
|
|
#if CAN_MODULE_ENABLE
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_finished_, Motor_Move_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
#endif
|
|
}
|
|
ZLOGI(TAG, "【FIRST DETECT】 MOVE FINISHED");
|
|
}
|
|
}
|
|
|
|
// 设置读取速率
|
|
void ECLMotor::startReadTimer() {
|
|
osTimerStart(readTimer, 50); // // 50毫秒
|
|
}
|
|
|
|
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_move_finished_ = false;
|
|
enc_val_ = 0;
|
|
|
|
is_home_init_ = false;
|
|
is_home_suc_ = false;
|
|
home_state_ = STATE_INIT;
|
|
isRunHomingTask = false;
|
|
|
|
// 创建定时器
|
|
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_);
|
|
}
|
|
|
|
// 启动定时器
|
|
startReadTimer();
|
|
}
|
|
|
|
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);
|
|
const int32_t pos = x_actual_;
|
|
osMutexRelease(mutex_);
|
|
const int32_t py_pos = ELCMotorHelper::isFlip(index_) ? -pos : pos;
|
|
return py_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;
|
|
osMutexRelease(mutex_);
|
|
}
|
|
|
|
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;
|
|
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;
|
|
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 (!is_move_finished_) {
|
|
motor->setVMax(this->target_speed_);
|
|
}
|
|
osMutexRelease(mutex_);
|
|
}
|
|
|
|
// 获取电机速度
|
|
int32_t ECLMotor::getRTSpeed() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
const int32_t spd = rt_speed_;
|
|
osMutexRelease(mutex_);
|
|
return spd;
|
|
}
|
|
|
|
// 获取电机运动状态
|
|
bool ECLMotor::isMoveFinished() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
bool is_move_finshed = is_move_finished_;
|
|
osMutexRelease(mutex_);
|
|
return is_move_finshed;
|
|
}
|
|
|
|
// 获取编码器位置
|
|
int32_t ECLMotor::getEncoderPosition() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
int32_t encPos = enc_val_;
|
|
osMutexRelease(mutex_);
|
|
const int32_t py_pos = ELCMotorHelper::isFlip(index_) ? -encPos : encPos;
|
|
return py_pos;
|
|
}
|
|
|
|
// 设置编码器位置
|
|
void ECLMotor::setEncoderPosition(int32_t encoderPos) {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
enc_val_ = encoderPos;
|
|
osMutexRelease(mutex_);
|
|
}
|
|
|
|
void ECLMotor::setMotorIndex(int32_t index) {
|
|
this->index_ = index;
|
|
}
|
|
|
|
bool ECLMotor::isHomeSuc() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
bool is_home_suc = is_home_suc_;
|
|
osMutexRelease(mutex_);
|
|
return is_home_suc;
|
|
}
|
|
|
|
void ECLMotor::runhomeSuc() {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
// 发送 CAN 信息
|
|
motor->setEncVal(0);
|
|
motor->setXACTUAL(0);
|
|
|
|
// 置标志位
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
this->is_home_init_ = true;
|
|
this->is_home_suc_ = true;
|
|
this->home_state_ = STATE_INIT;
|
|
isRunHomingTask = false;
|
|
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);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void ECLMotor::startMoveHome() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
this->is_home_suc_ = false;
|
|
isRunHomingTask = true;
|
|
osMutexRelease(mutex_);
|
|
|
|
const bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_);
|
|
// 当前是否在原点
|
|
if (isAtOrigin) {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
home_event_.event_type = EVENT_IN_ORIGIN;
|
|
osMutexRelease(mutex_);
|
|
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);
|
|
}
|
|
} else {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
home_event_.event_type = EVENT_START_HOMING;
|
|
osMutexRelease(mutex_);
|
|
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_ = seq_id;
|
|
}
|
|
|
|
void ECLMotor::homingTask(void *arg) {
|
|
ECLMotor *eclMotor = static_cast<ECLMotor *>(arg);
|
|
HomeEvent home_event;
|
|
|
|
while (true) {
|
|
osStatus status = osMessageQueueGet(eclMotor->homeEventQueue, &home_event, NULL, osWaitForever);
|
|
if(!eclMotor->isRunHomingTask) {
|
|
continue;
|
|
}
|
|
if (status == osOK) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(eclMotor->index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
#if 1
|
|
switch (home_event.event_type) {
|
|
case EVENT_START_HOMING: {
|
|
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: {
|
|
// if first enter
|
|
if(eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_MIDDLE) {
|
|
// 停止
|
|
// 中速向前10mm
|
|
osMutexAcquire(eclMotor->mutex_, osWaitForever);
|
|
eclMotor->home_state_ = STATE_LEAVE_ORIGIN_MIDDLE;
|
|
osMutexRelease(eclMotor->mutex_);
|
|
|
|
// 位置清零
|
|
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 中速向前10mm");
|
|
}
|
|
// if second enter
|
|
if (eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_LOW) {
|
|
// 位置清零
|
|
// 电机停止
|
|
// 回归原点成功
|
|
eclMotor->runhomeSuc();
|
|
ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] HOME SUC 回原点成功");
|
|
}
|
|
break;
|
|
}
|
|
case EVENT_IN_ORIGIN: {
|
|
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 中速向前10mm");
|
|
break;
|
|
}
|
|
case EVENT_MOTOR_MOVE_FINISHED: {
|
|
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);
|
|
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 低速回原点");
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
ZLOGI(TAG, "【HOME】 未处理事件类型 EVENT TYPE %d", home_event.event_type);
|
|
break;
|
|
}
|
|
#endif
|
|
} else {
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void ECLMotor::moveToHome() {
|
|
this->startMoveHome();
|
|
}
|
|
|
|
void ECLMotor::runZeroLimit(bool isEnter) {
|
|
// 处理回home 位逻辑
|
|
if (isRunHomingTask) {
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
|
|
if (isEnter) {
|
|
home_event_.event_type = EVENT_ORIGIN_ENTER;
|
|
motor->stop();
|
|
} else {
|
|
home_event_.event_type = 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();
|
|
{
|
|
#if CAN_MODULE_ENABLE
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_, Motor_Move_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
#endif
|
|
}
|
|
// 重置编码器 和 位置
|
|
motor->setXACTUAL(0);
|
|
motor->setEncVal(0);
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
this->x_actual_ = 0;
|
|
this->enc_val_ = 0;
|
|
this->target_pos_ = 0;
|
|
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() {
|
|
osMutexAcquire(mutex_, osWaitForever);
|
|
this->isRunHomingTask = false;
|
|
osMutexRelease(mutex_);
|
|
|
|
TMC51X0 *motor = AppHardware::ins()->getPump(index_);
|
|
if (motor == nullptr) {
|
|
return;
|
|
}
|
|
motor->stop();
|
|
|
|
// 发送CAN 消息
|
|
#if CAN_MODULE_ENABLE
|
|
CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Move_Finished, index_);
|
|
AppHardware::ins()->can0Controller.sendMessage(msg);
|
|
#endif
|
|
}
|
|
|
|
void ECLMotor::ECL_Rotate(int32_t speed, const bool is_flip) {
|
|
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);
|
|
}
|