temp

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);
+}
+

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

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

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();
 }

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__ */

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

optional/msp_MK32.c

@@ -17,7 +17,9 @@
 
 ******************************************************************************/
 #include "msp_MK32.h"
-#include "drv_interface.h"
+#include "BHBF_robot.h"
+#include <math.h>
+
 
 /*----------------------------------------------*
  * 外部变量说明                                 *
@@ -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;
+		}
+	}
+}

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,
+#define DF_MSP_Ti5Motor_StartID 0 
-//		int WaitTime)
+
-//{
+static int CANSendCommand(uint8_t id, uint8_t command, int32_t *content)
-//	OneByteCommand(id, 0x0B, Ti5_Motor_Controller, WaitTime);
+{
-//}
+    int len = (content != NULL) ? 5 : 1;
-//
+    char buf[10] = {0};
-//void Motor_GetFaultState(uint8_t id, FDCANHandler *Ti5_Motor_Controller,
+    buf[0] = id;
-//int WaitTime)
+    buf[1] = command;
-//{
+    if (content != NULL)
-//	OneByteCommand(id, 0x0A, Ti5_Motor_Controller, WaitTime); // set motor stop mode
+    {
-//
+        RD_MEMCPY(&buf[2], content, 4);        
-//}
+    }
-//
+    return rd_ComWrite(g_ptFDCAN1, (char *)buf, len);
-//void GetCSPByCommand(uint8_t id, FDCANHandler *Ti5_Motor_Controller,
+}
-//		int WaitTime)
+
-//{
+static void Motor_ClearFault(uint8_t id)
-//	OneByteCommand(id, 0x41, Ti5_Motor_Controller, WaitTime);
+{
-//}
+	CANSendCommand(id, 0x0B, NULL);
-//
+}
-//void Motor_SetVelocityModeAndTargetVelocity(uint8_t id,
+
-//                                            int32_t targetSpeed,
+static void Motor_GetFaultState(uint8_t id)
-//                                            FDCANHandler *Ti5_Motor_Controller,
+{
-//                                            int WaitTime)
+	CANSendCommand(id, 0x0A, NULL); // set motor stop mode
-//{
+
-//    FiveByteCommand(id, 0x1d, targetSpeed, Ti5_Motor_Controller, WaitTime); //Motor_SetVelocityModeAndTargetVelocity
+}
-//}
+
-
+static void GetCSPByCommand(uint8_t id)
-void TI5MOTOR_Init(void)
+{
+	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);
+	}
+}

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 <stdint.h>
+
+
+
+/*==============================================*
+ *      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__ */