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