LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

temp

master
Lizongdi 4 days ago
parent
4c5a4c253a
commit
40167b3325
9 changed files with 550 additions and 48 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 92
      bspMCU/BHBF_robot.c
  2. 5
      bspMCU/My_freeRTOS.c
  3. 2
      bspMCU/My_print.c
  4. 33
      bspMCU/drv_interface.c
  5. 106
      bspMCU/include/BHBF_robot.h
  6. 2
      bspMCU/include/drv_interface.h
  7. 47
      optional/msp_MK32.c
  8. 189
      optional/msp_TI5MOTOR.c
  9. 122
      optional/msp_TI5MOTOR.h

92
bspMCU/BHBF_robot.c
View File

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

5
bspMCU/My_freeRTOS.c
View File

@ -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

2
bspMCU/My_print.c
View File

@ -256,7 +256,7 @@ WEAK void Myprint_getchar(char *ch)
}
#ifdef USE_FREERTOS
osDelay(1);
Rd_Delay(1);
#endif
#ifdef USE_TIMER

33
bspMCU/drv_interface.c
View File

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

106
bspMCU/include/BHBF_robot.h
View File

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

2
bspMCU/include/drv_interface.h
View File

@ -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

47
optional/msp_MK32.c
View File

@ -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;
}
}
}

189
optional/msp_TI5MOTOR.c
View File

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

122
optional/msp_TI5MOTOR.h
View File

@ -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__ */
Write Preview
Loading…
Cancel
Save