8 changed files with 404 additions and 397 deletions
@ -1,89 +0,0 @@ |
|||
/******************************************************************************
|
|||
|
|||
版权所有 (C), 2018-2099, Radkil |
|||
|
|||
****************************************************************************** |
|||
文 件 名 : paint_robot_new.h |
|||
版 本 号 : 初稿 |
|||
作 者 : radkil |
|||
生成日期 : 2026年7月14日 |
|||
最近修改 : |
|||
功能描述 : paint_robot_new.c 的头文件 |
|||
|
|||
修改历史 : |
|||
1.日 期 : 2026年7月14日 |
|||
作 者 : radkil |
|||
修改内容 : 创建文件 |
|||
|
|||
******************************************************************************/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部变量说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 内部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 全局变量 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 模块级变量 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 常量定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 宏定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
#ifndef __PAINT_ROBOT_NEW_H__ |
|||
#define __PAINT_ROBOT_NEW_H__ |
|||
|
|||
|
|||
#ifdef __cplusplus |
|||
#if __cplusplus |
|||
extern "C"{ |
|||
#endif |
|||
#endif /* __cplusplus */ |
|||
|
|||
|
|||
/*==============================================*
|
|||
* include header files * |
|||
*----------------------------------------------*/ |
|||
#include <stdint.h> |
|||
|
|||
|
|||
|
|||
/*==============================================*
|
|||
* constants or macros define * |
|||
*----------------------------------------------*/ |
|||
|
|||
|
|||
/*==============================================*
|
|||
* project-wide global variables * |
|||
*----------------------------------------------*/ |
|||
|
|||
|
|||
|
|||
/*==============================================*
|
|||
* routines' or functions' implementations * |
|||
*----------------------------------------------*/ |
|||
|
|||
|
|||
#ifdef __cplusplus |
|||
#if __cplusplus |
|||
} |
|||
#endif |
|||
#endif /* __cplusplus */ |
|||
|
|||
|
|||
#endif /* __PAINT_ROBOT_NEW_H__ */ |
|||
@ -0,0 +1,140 @@ |
|||
/******************************************************************************
|
|||
|
|||
版权所有 (C), 2018-2099, Radkil |
|||
|
|||
****************************************************************************** |
|||
文 件 名 : paint_robot_new_MK32.c |
|||
版 本 号 : 初稿 |
|||
作 者 : radkil |
|||
生成日期 : 2026年8月21日 |
|||
最近修改 : |
|||
功能描述 : 新喷涂机器人遥控器逻辑 |
|||
|
|||
修改历史 : |
|||
1.日 期 : 2026年8月21日 |
|||
作 者 : radkil |
|||
修改内容 : 创建文件 |
|||
|
|||
******************************************************************************/ |
|||
#include "BHBF.h" |
|||
#include "Protobuf/PSource/msp_MK32.pb.h" |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部变量说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 内部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 全局变量 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 模块级变量 * |
|||
*----------------------------------------------*/ |
|||
static SP_MSP_MK32_Button RB_MK32; |
|||
|
|||
/*----------------------------------------------*
|
|||
* 常量定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 宏定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
int check_MK32(char *_pBuffer, uint32_t _iSize) |
|||
{ |
|||
if (_iSize < 25) return 0; // 数据不足25字节，不读[24]不误判，等下一包
|
|||
if (_pBuffer[0] != 0x0f) return -1; |
|||
if (_pBuffer[24] != 0x00) return -1; |
|||
return 25; |
|||
} |
|||
|
|||
void decode_MK32(const char *buf, uint32_t _iSize) |
|||
{ |
|||
int32_t CH[16]; |
|||
int Start_byte = 0; |
|||
|
|||
CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF); |
|||
CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF); |
|||
CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2 |
|||
| buf[Start_byte + 5] << 10) & 0x07FF); |
|||
CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF); |
|||
CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF); |
|||
CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1 |
|||
| buf[Start_byte + 9] << 9) & 0x07FF); |
|||
CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF); |
|||
CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF); |
|||
CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF); |
|||
CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF); |
|||
CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2 |
|||
| buf[Start_byte + 16] << 10) & 0x07FF); |
|||
CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF); |
|||
CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF); |
|||
CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1 |
|||
| buf[Start_byte + 20] << 9) & 0x07FF); |
|||
CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF); |
|||
CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF); |
|||
|
|||
// 按键数值转换：1050为中间值，272-1712
|
|||
for (int i = 0; i < 16; i++) |
|||
{ |
|||
CH[i] = (int32_t)((CH[i] - 992) * 1.388889); |
|||
} |
|||
|
|||
RD_MEMCPY(&(RB_MK32.CH0_RY_H), CH, sizeof(CH)); |
|||
|
|||
if (buf[23] == 0) |
|||
{ |
|||
RB_MK32.IsOnline = 1; |
|||
} |
|||
else |
|||
{ |
|||
RB_MK32.IsOnline = 0; |
|||
} |
|||
RB_MK32.RxIndex++; |
|||
|
|||
int angle; |
|||
angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI; |
|||
|
|||
if(RB_MK32.CH5_SB == 0) |
|||
{ |
|||
// 前进
|
|||
if((angle >= 45) && (angle <= 135)) |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); |
|||
} |
|||
// 后退
|
|||
else if((angle >= -135) && (angle < -45)) |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); |
|||
} |
|||
else |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); |
|||
} |
|||
} |
|||
else |
|||
{ |
|||
// 前进
|
|||
if((angle >= 45) && (angle <= 135)) |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); |
|||
} |
|||
// 后退
|
|||
else if((angle >= -135) && (angle < -45)) |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); |
|||
} |
|||
else |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); |
|||
} |
|||
} |
|||
} |
|||
@ -0,0 +1,243 @@ |
|||
/******************************************************************************
|
|||
|
|||
版权所有 (C), 2018-2099, Radkil |
|||
|
|||
****************************************************************************** |
|||
文 件 名 : paint_robot_new_motors.c |
|||
版 本 号 : 初稿 |
|||
作 者 : radkil |
|||
生成日期 : 2026年8月21日 |
|||
最近修改 : |
|||
功能描述 : 喷涂机器人电机逻辑 |
|||
|
|||
修改历史 : |
|||
1.日 期 : 2026年8月21日 |
|||
作 者 : radkil |
|||
修改内容 : 创建文件 |
|||
|
|||
******************************************************************************/ |
|||
#include "BHBF.h" |
|||
#include "motor_manager.h" |
|||
#include "msp_motor_leisai.h" |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部变量说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 内部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 全局变量 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 模块级变量 * |
|||
*----------------------------------------------*/ |
|||
static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; |
|||
static uint32_t g_uiMotorModuleID = 0; |
|||
|
|||
/*----------------------------------------------*
|
|||
* 常量定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 宏定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
static void Motor_ModuleHandler(const Msg_t *pstMsg) |
|||
{ |
|||
if (NULL == pstMsg) |
|||
{ |
|||
return; |
|||
} |
|||
|
|||
switch (pstMsg->m_uiMsgID) |
|||
{ |
|||
case MOTOR_CMD_SET_SPEED: |
|||
{ |
|||
if (pstMsg->m_uiDataLen >= sizeof(Motor_CmdData_t)) |
|||
{ |
|||
Motor_CmdData_t *pstData = (Motor_CmdData_t *)pstMsg->m_aucData; |
|||
if (pstData->m_ucMotorIndex < 2 && g_apstMotors[pstData->m_ucMotorIndex] != NULL) |
|||
{ |
|||
MotorMgr_SetTargetSpeed(g_apstMotors[pstData->m_ucMotorIndex], pstData->m_iValue); |
|||
} |
|||
} |
|||
break; |
|||
} |
|||
case MOTOR_CMD_STOP: |
|||
{ |
|||
for (int i = 0; i < 2; i++) |
|||
{ |
|||
if (g_apstMotors[i] != NULL) |
|||
{ |
|||
MotorMgr_SetTargetSpeed(g_apstMotors[i], 0); |
|||
} |
|||
} |
|||
break; |
|||
} |
|||
case MOTOR_CMD_SET_HOME: |
|||
{ |
|||
if (pstMsg->m_uiDataLen >= sizeof(Motor_CmdData_t)) |
|||
{ |
|||
Motor_CmdData_t *pstData = (Motor_CmdData_t *)pstMsg->m_aucData; |
|||
if (pstData->m_ucMotorIndex < 2 && g_apstMotors[pstData->m_ucMotorIndex] != NULL) |
|||
{ |
|||
MotorMgr_SetHome(g_apstMotors[pstData->m_ucMotorIndex]); |
|||
} |
|||
} |
|||
break; |
|||
} |
|||
case MOTOR_CMD_GET_STATUS: |
|||
{ |
|||
// 回复状态给请求者
|
|||
Motor_StatusData_t stStatus; |
|||
for (int i = 0; i < 2; i++) |
|||
{ |
|||
if (g_apstMotors[i] != NULL) |
|||
{ |
|||
const Motor_Data_t *pstData = MotorMgr_GetData(g_apstMotors[i]); |
|||
if (pstData != NULL) |
|||
{ |
|||
stStatus.m_ucMotorIndex = i; |
|||
stStatus.m_iPosition = pstData->m_iRealPosition; |
|||
stStatus.m_iVelocity = pstData->m_iRealVelocity; |
|||
stStatus.m_uiFaultCode = pstData->m_uiFaultCode; |
|||
|
|||
// 回复给请求者
|
|||
MsgCenter_SendTo(MODULE_NAME_RBCORE, 0x10, &stStatus, sizeof(stStatus)); |
|||
} |
|||
} |
|||
} |
|||
break; |
|||
} |
|||
default: |
|||
break; |
|||
} |
|||
} |
|||
|
|||
// 雷赛电机CAN响应检查函数
|
|||
// 雷赛响应ID范围：0x581 - 0x5FF (0x580 + MotorID)
|
|||
static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize) |
|||
{ |
|||
if (NULL == _pBuffer || _iSize < 4) |
|||
{ |
|||
return 0; |
|||
} |
|||
|
|||
// 从buffer中提取CAN ID（前4字节）
|
|||
uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) |
|||
| ((_pBuffer[1] & 0xFF) << 8) |
|||
| ((_pBuffer[2] & 0xFF) << 16) |
|||
| ((_pBuffer[3] & 0xFF) << 24); |
|||
|
|||
// 雷赛响应ID范围：0x581 - 0x5FF
|
|||
if (uiFrameID >= 0x581 && uiFrameID <= 0x5FF) |
|||
{ |
|||
return 8 + 4; // 数据长度8 + CAN ID长度4
|
|||
} |
|||
|
|||
return 0; |
|||
} |
|||
|
|||
// 雷赛电机CAN响应解码函数
|
|||
static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize) |
|||
{ |
|||
if (NULL == _pBuffer || _iSize < 12) // 4字节CAN ID + 8字节数据
|
|||
{ |
|||
return; |
|||
} |
|||
|
|||
// 从buffer中提取CAN ID
|
|||
uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF) |
|||
| ((_pBuffer[1] & 0xFF) << 8) |
|||
| ((_pBuffer[2] & 0xFF) << 16) |
|||
| ((_pBuffer[3] & 0xFF) << 24); |
|||
|
|||
// 提取电机ID (0x580 + MotorID)
|
|||
uint8_t ucMotorID = (uint8_t)(uiFrameID - 0x580); |
|||
|
|||
// 调用电机管理模块解析响应
|
|||
MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8); |
|||
} |
|||
|
|||
static void MotorInit(void) |
|||
{ |
|||
// 初始化FDCAN1，使用雷赛电机的回调
|
|||
TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0); |
|||
g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData); |
|||
CAN_IT_init(g_ptCAN1); |
|||
|
|||
// 初始化电机管理模块
|
|||
MotorMgr_Init(); |
|||
|
|||
// 配置左轮电机
|
|||
Motor_Config_t stLeftMotorConfig = { |
|||
.m_ucMotorID = 1, |
|||
.m_uiPulsePerRound = 10000, |
|||
.m_uiReductionRatio = 70, |
|||
.m_fWheelDiameter = 0.26f |
|||
}; |
|||
g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); |
|||
|
|||
// 配置右轮电机
|
|||
Motor_Config_t stRightMotorConfig = { |
|||
.m_ucMotorID = 2, |
|||
.m_uiPulsePerRound = 10000, |
|||
.m_uiReductionRatio = 70, |
|||
.m_fWheelDiameter = 0.26f |
|||
}; |
|||
g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); |
|||
|
|||
g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler); |
|||
} |
|||
|
|||
void MotorTask(void *argument) |
|||
{ |
|||
MotorInit(); |
|||
|
|||
MotorMgr_ResetAll(g_apstMotors[0]); |
|||
Rd_Delay(500); |
|||
MotorMgr_ResetAll(g_apstMotors[1]); |
|||
Rd_Delay(500); |
|||
|
|||
int iCount = 6; |
|||
while(iCount--) |
|||
{ |
|||
MotorMgr_ActivateAll(g_apstMotors[0]); |
|||
Rd_Delay(500); |
|||
MotorMgr_ActivateAll(g_apstMotors[1]); |
|||
Rd_Delay(500); |
|||
} |
|||
|
|||
MotorMgr_SetHome(g_apstMotors[0]); |
|||
MotorMgr_SetHome(g_apstMotors[1]); |
|||
MotorMgr_SpeedModeInit(g_apstMotors[0]); |
|||
MotorMgr_SpeedModeInit(g_apstMotors[1]); |
|||
|
|||
while(1) |
|||
{ |
|||
// 处理消息
|
|||
MsgCenter_ProcessWait(g_uiMotorModuleID, 2); |
|||
|
|||
for (int i = 0; i < MOTOR_INSTANCE_MAX; i++) |
|||
{ |
|||
if (g_apstMotors[i] != NULL) |
|||
{ |
|||
// 读取位置、速度、故障
|
|||
MotorMgr_RequestPosition(g_apstMotors[i]); |
|||
MotorMgr_RequestFault(g_apstMotors[i]); |
|||
MotorMgr_RequestVelocity(g_apstMotors[i]); |
|||
MotorMgr_SetTargetSpeed(g_apstMotors[i], 50000); |
|||
} |
|||
} |
|||
|
|||
Rd_Delay(2); // 2ms周期
|
|||
} |
|||
} |
|||
Loading…
Reference in new issue