From 40167b33254270bf7d34d8aa038994de1bfc8657 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Tue, 9 Jun 2026 09:43:24 +0800 Subject: [PATCH] temp --- bspMCU/BHBF_robot.c | 92 ++++++++++++++++ bspMCU/My_freeRTOS.c | 5 + bspMCU/My_print.c | 2 +- bspMCU/drv_interface.c | 33 +++--- bspMCU/include/BHBF_robot.h | 106 ++++++++++++++++++ bspMCU/include/drv_interface.h | 2 + optional/msp_MK32.c | 47 +++++++- optional/msp_TI5MOTOR.c | 189 ++++++++++++++++++++++++++++----- optional/msp_TI5MOTOR.h | 122 +++++++++++++++++++++ 9 files changed, 550 insertions(+), 48 deletions(-) create mode 100644 bspMCU/BHBF_robot.c create mode 100644 bspMCU/include/BHBF_robot.h create mode 100644 optional/msp_TI5MOTOR.h diff --git a/bspMCU/BHBF_robot.c b/bspMCU/BHBF_robot.c new file mode 100644 index 0000000..a745c9b --- /dev/null +++ b/bspMCU/BHBF_robot.c @@ -0,0 +1,92 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : BHBF_robot.c + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年6月8日 + 最近修改 : + 功能描述 : 机器人主程序 + + 修改历史 : + 1.日 期 : 2026年6月8日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ +#include "BHBF_robot.h" +#include "common.h" + +#include "cmsis_os.h" +#include "FreeRTOS.h" +#include "task.h" + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ +GV_struct_define GV = {0}; + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ +GV_struct_define *GetGV(void) +{ + static GV_struct_define tmpGV = {0}; + RD_MEMSET(&tmpGV, 0, sizeof(tmpGV)); + RD_MEMCPY(&tmpGV, &GV, sizeof(GV_struct_define)); + return &tmpGV; +} + +void SetGV(GV_struct_define *_pGV) +{ + RD_MEMCPY(&GV, _pGV, sizeof(GV_struct_define)); +} + +void GF_FsmStart(GF_CMD GFCmd) +{ + +} + +void GF_Dispatch(void *argument) +{ + while(1) + { + GF_CMD GFCmd = MK32_Task(GV.m_iWorkMode); + GF_FsmStart(GFCmd); + } +} + +void GF_Init(void) +{ + Drv_InterfaceInit(); + + const osThreadAttr_t GF_Dispatch_attributes = { + .name = "GF_Task", + .stack_size = 1024, + .priority = (osPriority_t) osPriorityRealtime1, + }; + (void)osThreadNew(GF_Dispatch, NULL, &GF_Dispatch_attributes); +} + diff --git a/bspMCU/My_freeRTOS.c b/bspMCU/My_freeRTOS.c index be94cf3..a7ac3db 100644 --- a/bspMCU/My_freeRTOS.c +++ b/bspMCU/My_freeRTOS.c @@ -156,6 +156,11 @@ void Rd_MutexUnlock(void *_pMutex, const char *_pcFunc, int *_piCnt) (void)xTaskResumeAll(); } +void Rd_Delay(uint32_t Delay) +{ + osDelay(Delay); +} + void MyTask_Init(void) { #ifndef USE_TALNET diff --git a/bspMCU/My_print.c b/bspMCU/My_print.c index 170442c..0aedec5 100644 --- a/bspMCU/My_print.c +++ b/bspMCU/My_print.c @@ -256,7 +256,7 @@ WEAK void Myprint_getchar(char *ch) } #ifdef USE_FREERTOS - osDelay(1); + Rd_Delay(1); #endif #ifdef USE_TIMER diff --git a/bspMCU/drv_interface.c b/bspMCU/drv_interface.c index f748d83..7cb244f 100644 --- a/bspMCU/drv_interface.c +++ b/bspMCU/drv_interface.c @@ -16,7 +16,7 @@ 修改内容 : 创建文件 ******************************************************************************/ -#include "drv_interface.h" +#include "BHBF_robot.h" #include "cmsis_os.h" #include "FreeRTOS.h" @@ -118,12 +118,12 @@ int RS485_4_Send(char *_pBuffer, uint32_t _iSize) int FDCAN1_Send(char *_pBuffer, uint32_t _iSize) { - return CAN_TX_FIFOQ(g_ptFDCAN1, _pBuffer[0], &_pBuffer[4], _iSize); + return CAN_TX_FIFOQ(g_ptFDCAN1, _pBuffer[0], &_pBuffer[1], _iSize); } int FDCAN2_Send(char *_pBuffer, uint32_t _iSize) { - return CAN_TX_FIFOQ(g_ptFDCAN2, _pBuffer[0], &_pBuffer[4], _iSize); + return CAN_TX_FIFOQ(g_ptFDCAN2, _pBuffer[0], &_pBuffer[1], _iSize); } #ifndef CONFIG_UART_IT_IDLEDMA @@ -193,7 +193,7 @@ void Read_RS485_1(void *argument) __disable_irq(); rd_ComRead(g_ptRS485_1, pcBuffer, 25); __enable_irq(); - osDelay(1); + Rd_Delay(1); } } @@ -202,31 +202,26 @@ void SendAll_Task(void *argument) while(1) { rd_ComSendProc(g_ptRS485_1); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptRS485_2); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptRS485_3); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptRS485_4); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptLTE_7S0); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptE28_SBUS); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptInterCall); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptFDCAN1); - osDelay(1); + Rd_Delay(1); rd_ComSendProc(g_ptFDCAN2); - osDelay(1); + Rd_Delay(1); } } -WEAK void MK32_Init(void) -{ - return; -} - void Drv_InterfaceInit(void) { MK32_Init(); @@ -244,4 +239,6 @@ void Drv_InterfaceInit(void) .priority = (osPriority_t) osPriorityRealtime, }; (void)osThreadNew(SendAll_Task, NULL, &send_task_attributes); + + MoveWheel_Init(); } diff --git a/bspMCU/include/BHBF_robot.h b/bspMCU/include/BHBF_robot.h new file mode 100644 index 0000000..c9b5ef2 --- /dev/null +++ b/bspMCU/include/BHBF_robot.h @@ -0,0 +1,106 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : BHBF_robot.h + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年6月8日 + 最近修改 : + 功能描述 : BHBF_robot.c 的头文件 + + 修改历史 : + 1.日 期 : 2026年6月8日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +#ifndef __BHBF_ROBOT_H__ +#define __BHBF_ROBOT_H__ + + +#ifdef __cplusplus +#if __cplusplus +extern "C"{ +#endif +#endif /* __cplusplus */ + + +/*==============================================* + * include header files * + *----------------------------------------------*/ +#include "drv_interface.h" +#include "rd_time.h" + + +/*==============================================* + * constants or macros define * + *----------------------------------------------*/ +typedef enum { + GF_CMD_DEFAULT_NULL = 0, + GF_CMD_STOP_ALL, + GF_CMD_MANUAL_FORWARD, + GF_CMD_MANUAL_BACKWARD, + GF_CMD_AUTO_FORWARD +} GF_CMD; + +typedef struct _GV_struct_define +{ + int m_iWorkMode; + GF_CMD m_eGFCmd; + int32_t m_iMoveSpeed[2];//索引1对应左轮，索引2对应右轮 +} GV_struct_define; + +/*==============================================* + * project-wide global variables * + *----------------------------------------------*/ + + + +/*==============================================* + * routines' or functions' implementations * + *----------------------------------------------*/ +// 待实现接口 +void MK32_Init(void); +GF_CMD MK32_Task(int mode); +void MoveWheel_Init(void); +void MoveWheel_Task(int32_t Target_Velcity[]); + +#ifdef __cplusplus +#if __cplusplus +} +#endif +#endif /* __cplusplus */ + + +#endif /* __BHBF_ROBOT_H__ */ diff --git a/bspMCU/include/drv_interface.h b/bspMCU/include/drv_interface.h index 1257857..510530d 100644 --- a/bspMCU/include/drv_interface.h +++ b/bspMCU/include/drv_interface.h @@ -99,6 +99,8 @@ extern int RS485_1_Send(char *_pBuffer, uint32_t _iSize); extern int RS485_2_Send(char *_pBuffer, uint32_t _iSize); extern int RS485_3_Send(char *_pBuffer, uint32_t _iSize); extern int RS485_4_Send(char *_pBuffer, uint32_t _iSize); +extern void Drv_InterfaceInit(void); + #ifdef __cplusplus #if __cplusplus diff --git a/optional/msp_MK32.c b/optional/msp_MK32.c index 9154c46..cab113a 100644 --- a/optional/msp_MK32.c +++ b/optional/msp_MK32.c @@ -17,7 +17,9 @@ ******************************************************************************/ #include "msp_MK32.h" -#include "drv_interface.h" +#include "BHBF_robot.h" +#include + /*----------------------------------------------* * 外部变量说明 * @@ -123,3 +125,46 @@ void MK32_Init(void) g_ptE28_SBUS = rd_ComCreate(check_MK32Data, decode_MK32Data, E28_SBUS_Send, ptUartUserData->m_buf_size, ptUartUserData); UART_IT_init(g_ptE28_SBUS); } + +GF_CMD MK32_Task(int mode) +{ + 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)) + { + if (0 == mode) return GF_CMD_MANUAL_FORWARD; + else return GF_CMD_AUTO_FORWARD; + } + // 后退 + else if((angle >= -135) && (angle < -45)) + { + return GF_CMD_MANUAL_BACKWARD; + } + else + { + return GF_CMD_STOP_ALL; + } + } + else + { + // 前进 + if((angle >= 45) && (angle <= 135)) + { + if (0 == mode) return GF_CMD_MANUAL_FORWARD; + else return GF_CMD_AUTO_FORWARD; + } + // 后退 + else if((angle >= -135) && (angle < -45)) + { + return GF_CMD_MANUAL_BACKWARD; + } + else + { + return GF_CMD_STOP_ALL; + } + } +} diff --git a/optional/msp_TI5MOTOR.c b/optional/msp_TI5MOTOR.c index 200e1a0..0eb29b5 100644 --- a/optional/msp_TI5MOTOR.c +++ b/optional/msp_TI5MOTOR.c @@ -16,6 +16,8 @@ 修改内容 : 创建文件 ******************************************************************************/ +#include "msp_TI5MOTOR.h" +#include "BHBF_robot.h" /*----------------------------------------------* * 外部变量说明 * @@ -36,6 +38,9 @@ /*----------------------------------------------* * 模块级变量 * *----------------------------------------------*/ +static MotorParameters Motor[4]; +static int32_t Motor_ID_Errors[7] = { 0 }; +static CSP tempCSP; /*----------------------------------------------* * 常量定义 * @@ -45,34 +50,162 @@ * 宏定义 * *----------------------------------------------*/ -//void Motor_ClearFault(uint8_t id, FDCANHandler *Ti5_Motor_Controller, -// int WaitTime) -//{ -// OneByteCommand(id, 0x0B, Ti5_Motor_Controller, WaitTime); -//} -// -//void Motor_GetFaultState(uint8_t id, FDCANHandler *Ti5_Motor_Controller, -//int WaitTime) -//{ -// OneByteCommand(id, 0x0A, Ti5_Motor_Controller, WaitTime); // set motor stop mode -// -//} -// -//void GetCSPByCommand(uint8_t id, FDCANHandler *Ti5_Motor_Controller, -// int WaitTime) -//{ -// OneByteCommand(id, 0x41, Ti5_Motor_Controller, WaitTime); -//} -// -//void Motor_SetVelocityModeAndTargetVelocity(uint8_t id, -// int32_t targetSpeed, -// FDCANHandler *Ti5_Motor_Controller, -// int WaitTime) -//{ -// FiveByteCommand(id, 0x1d, targetSpeed, Ti5_Motor_Controller, WaitTime); //Motor_SetVelocityModeAndTargetVelocity -//} - -void TI5MOTOR_Init(void) +#define DF_MSP_Ti5Motor_StartID 0 + +static int CANSendCommand(uint8_t id, uint8_t command, int32_t *content) +{ + int len = (content != NULL) ? 5 : 1; + char buf[10] = {0}; + buf[0] = id; + buf[1] = command; + if (content != NULL) + { + RD_MEMCPY(&buf[2], content, 4); + } + return rd_ComWrite(g_ptFDCAN1, (char *)buf, len); +} + +static void Motor_ClearFault(uint8_t id) +{ + CANSendCommand(id, 0x0B, NULL); +} + +static void Motor_GetFaultState(uint8_t id) +{ + CANSendCommand(id, 0x0A, NULL); // set motor stop mode + +} + +static void GetCSPByCommand(uint8_t id) +{ + CANSendCommand(id, 0x41, NULL); +} + +static void Motor_SetVelocityModeAndTargetVelocity(uint8_t id, int32_t targetSpeed) +{ + CANSendCommand(id, 0x1d, &targetSpeed); //Motor_SetVelocityModeAndTargetVelocity +} + +static int check_TI5MOTOR(char *buffer, uint32_t length) +{ + TCANUserData *ptCANUserData = (TCANUserData *)g_ptFDCAN1->m_pUserData; + uint8_t ID_A_T = (uint8_t)ptCANUserData->m_canrx->Identifier; + int32_t Function_code = 0; + + if (ID_A_T >= DF_MSP_Ti5Motor_StartID + && ID_A_T < DF_MSP_Ti5Motor_StartID + 7) + { + (Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).RxIndex++; + } + + switch (Function_code) + { + case 3: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Run_Mode, + &buffer[1], length - 1); + break; + case 4: //ma + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Current, + &buffer[1], length - 1); + + break; + case 5: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Target_Current, + &buffer[1], length - 1); + + break; + case 6: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Velcity, + &buffer[1], length - 1); + + break; + case 7: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Target_Velcity, + &buffer[1], length - 1); + + break; + case 8: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Position, + &buffer[1], length - 1); + + break; + case 9: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Target_Position, + &buffer[1], length - 1); + break; + case 10: + + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).ERROR_Flag, + &buffer[1], length - 1); + + memcpy(&(Motor_ID_Errors[ID_A_T - DF_MSP_Ti5Motor_StartID]), + &buffer[1], length - 1); + break; + case 49: + memcpy( + &(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Temperature_Motor, + &buffer[1], length - 1); + break; + case 50: + memcpy(&(Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Temperature_PCB, + &buffer[1], length - 1); + break; + + case 65: + case 66: + case 67: + case 68: + //电流 MA + // case 65-68 均返回CSP + //tempCSP=&buffer[0]; + memcpy(&tempCSP, buffer, 8); + (Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Current = + tempCSP.Current;//单位为mA + (Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Velcity = + tempCSP.Velocity;//转化为度每秒公式：(返回值/100/减速比)*360 + (Motor[ID_A_T - DF_MSP_Ti5Motor_StartID]).Position = + tempCSP.Position;//转化为减速机角度公式：(返回值/65536/减速比)*360 + return 8; + default: + return -1; + } + return 1; +} + +static void decode_TI5MOTOR(const char *_pBuffer, uint32_t _iSize) { return; } + +void MoveWheel_Init(void) +{ + TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 8, 0); + g_ptFDCAN1 = rd_ComCreate(check_TI5MOTOR, decode_TI5MOTOR, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData); + CAN_IT_init(g_ptFDCAN1); + + for (int i = 1; i < 5; i++) + { + GetCSPByCommand(i); + Rd_Delay(4); + Motor_ClearFault(i); + Rd_Delay(4); + } +} + +void MoveWheel_Task(int32_t Target_Velcity[]) +{ + for (int i = 1; i < 5; i++) + { + Motor_ClearFault(i); + Rd_Delay(4); + Motor_GetFaultState(i); + Rd_Delay(4); + GetCSPByCommand(i); + Rd_Delay(4); + } + for (int i = 1; i < 5; i++) + { + Motor_SetVelocityModeAndTargetVelocity(i, Target_Velcity[i]); + Rd_Delay(4); + } +} diff --git a/optional/msp_TI5MOTOR.h b/optional/msp_TI5MOTOR.h new file mode 100644 index 0000000..195f931 --- /dev/null +++ b/optional/msp_TI5MOTOR.h @@ -0,0 +1,122 @@ +/****************************************************************************** + + 版权所有 (C), 2018-2099, Radkil + + ****************************************************************************** + 文 件 名 : msp_TI5MOTOR.h + 版 本 号 : 初稿 + 作 者 : radkil + 生成日期 : 2026年6月8日 + 最近修改 : + 功能描述 : msp_TI5MOTOR.c 的头文件 + + 修改历史 : + 1.日 期 : 2026年6月8日 + 作 者 : radkil + 修改内容 : 创建文件 + +******************************************************************************/ + +/*----------------------------------------------* + * 外部变量说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 外部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 内部函数原型说明 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 全局变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 模块级变量 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 常量定义 * + *----------------------------------------------*/ + +/*----------------------------------------------* + * 宏定义 * + *----------------------------------------------*/ + +#ifndef __MSP_TI5MOTOR_H__ +#define __MSP_TI5MOTOR_H__ + + +#ifdef __cplusplus +#if __cplusplus +extern "C"{ +#endif +#endif /* __cplusplus */ + + +/*==============================================* + * include header files * + *----------------------------------------------*/ +#include + + + +/*==============================================* + * constants or macros define * + *----------------------------------------------*/ +typedef struct _MotorParameters { + int32_t MotorID; + int32_t RxIndex; + int32_t Run_Mode; + int32_t Current; + int32_t Target_Current; + int32_t Velcity; + int32_t Target_Velcity; + int32_t Position; + int32_t Target_Position; + int32_t ERROR_Flag; + int32_t Temperature_Motor; + int32_t Temperature_PCB; + int32_t AccTime; + int32_t DecTime; + int32_t EncoderOffset; /* 53 83 设置位置偏移 int32_t "设置偏移值和目标位置 +（目标位置=编码器位置 - 偏移值）" */ +} MotorParameters; + +#pragma pack (2) /*指定按2字节对齐*/ +typedef struct _CSP +{ + int16_t Current; + int16_t Velocity; + int32_t Position; +} CSP; + +typedef struct _TSP +{ + int32_t Torque; + int32_t Velocity; + int32_t Position; +} TSP; + +#pragma pack () /*取消指定对齐，恢复缺省对齐*/ + +/*==============================================* + * project-wide global variables * + *----------------------------------------------*/ + + + +/*==============================================* + * routines' or functions' implementations * + *----------------------------------------------*/ + +#ifdef __cplusplus +#if __cplusplus +} +#endif +#endif /* __cplusplus */ + + +#endif /* __MSP_TI5MOTOR_H__ */