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.
123 lines
3.7 KiB
123 lines
3.7 KiB
2 weeks ago
|
/******************************************************************************
|
||
|
|
|
||
|
|
版权所有 (C), 2018-2099, Radkil
|
||
|
|
|
||
|
|
******************************************************************************
|
||
|
|
文 件 名 : paint_robot_new.c
|
||
|
|
版 本 号 : 初稿
|
||
|
|
作 者 : radkil
|
||
|
|
生成日期 : 2026年7月14日
|
||
|
|
最近修改 :
|
||
|
|
功能描述 : 新版喷漆主逻辑
|
||
|
|
|
||
|
|
修改历史 :
|
||
|
|
1.日 期 : 2026年7月14日
|
||
|
|
作 者 : radkil
|
||
|
|
修改内容 : 创建文件
|
||
|
|
|
||
|
|
******************************************************************************/
|
||
|
|
#include "paint_robot_new.h"
|
||
|
|
#include "com.h"
|
||
|
|
#include "rd_time.h"
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 外部变量说明 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
extern TComCtrl *g_ptFDCAN1;
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 外部函数原型说明 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 内部函数原型说明 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 全局变量 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
static Motor_Instance_t *g_pstLeftMotor; // 左轮电机实例
|
||
|
|
static Motor_Instance_t *g_pstRightMotor; // 右轮电机实例
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 模块级变量 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 常量定义 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 宏定义 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
void MotorInit(void)
|
||
|
|
{
|
||
|
|
// 初始化电机管理模块
|
||
|
|
MotorMgr_Init();
|
||
|
|
|
||
|
|
|
||
|
|
// 配置左轮电机
|
||
|
|
Motor_Config_t stLeftMotorConfig = {
|
||
|
|
.m_ucMotorID = 1,
|
||
|
|
.m_uiPulsePerRound = 10000,
|
||
|
|
.m_uiReductionRatio = 70,
|
||
|
|
.m_fWheelDiameter = 0.26f
|
||
|
|
};
|
||
|
|
g_pstLeftMotor = MotorMgr_Create(&stLeftMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);
|
||
|
|
|
||
|
|
// 配置右轮电机
|
||
|
|
Motor_Config_t stRightMotorConfig = {
|
||
|
|
.m_ucMotorID = 2,
|
||
|
|
.m_uiPulsePerRound = 10000,
|
||
|
|
.m_uiReductionRatio = 70,
|
||
|
|
.m_fWheelDiameter = 0.26f
|
||
|
|
};
|
||
|
|
g_pstRightMotor = MotorMgr_Create(&stRightMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);
|
||
|
|
}
|
||
|
|
|
||
|
|
void MotorTask(void *argument)
|
||
|
|
{
|
||
|
|
// 等待系统稳定
|
||
|
|
Rd_Delay(1000);
|
||
|
|
|
||
|
|
// 复位并激活所有电机
|
||
|
|
if (g_pstLeftMotor != NULL)
|
||
|
|
{
|
||
|
|
MotorMgr_ResetAll(g_pstLeftMotor);
|
||
|
|
Rd_Delay(500);
|
||
|
|
MotorMgr_ActivateAll(g_pstLeftMotor);
|
||
|
|
Rd_Delay(500);
|
||
|
|
MotorMgr_SetHome(g_pstLeftMotor);
|
||
|
|
Rd_Delay(100);
|
||
|
|
MotorMgr_SpeedModeInit(g_pstLeftMotor);
|
||
|
|
}
|
||
|
|
|
||
|
|
while(1)
|
||
|
|
{
|
||
|
|
if (g_pstLeftMotor != NULL)
|
||
|
|
{
|
||
|
|
// 读取位置、速度、故障
|
||
|
|
MotorMgr_RequestPosition(g_pstLeftMotor);
|
||
|
|
MotorMgr_RequestFault(g_pstLeftMotor);
|
||
|
|
MotorMgr_RequestVelocity(g_pstLeftMotor);
|
||
|
|
|
||
|
|
// 设置目标速度
|
||
|
|
MotorMgr_SetTargetSpeed(g_pstLeftMotor, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (g_pstRightMotor != NULL)
|
||
|
|
{
|
||
|
|
// 读取位置、速度、故障
|
||
|
|
MotorMgr_RequestPosition(g_pstRightMotor);
|
||
|
|
MotorMgr_RequestFault(g_pstRightMotor);
|
||
|
|
MotorMgr_RequestVelocity(g_pstRightMotor);
|
||
|
|
|
||
|
|
// 设置目标速度
|
||
|
|
MotorMgr_SetTargetSpeed(g_pstRightMotor, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
Rd_Delay(2); // 2ms周期
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|