You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
189 lines
4.3 KiB
189 lines
4.3 KiB
/* Private includes ----------------------------------------------------------*/
|
|
/* USER CODE BEGIN Includes */
|
|
#include "BHBF_ROBOT.h"
|
|
#include "bsp_FDCAN.h"
|
|
#include <bsp_client_setting.h>
|
|
#include <bsp_tempature.h>
|
|
#include "paint_robot_old.h"
|
|
//#include "msp_DMKE_485_Motor.h"
|
|
/* USER CODE END Includes */
|
|
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
/* USER CODE BEGIN PTD */
|
|
void GF_Robot_Init();
|
|
/* USER CODE END PTD */
|
|
|
|
/* Private define ------------------------------------------------------------*/
|
|
/* USER CODE BEGIN PD */
|
|
|
|
#define RS485_1_WaitTime 6
|
|
|
|
#define RS485_2_WaitTime 6
|
|
#define RS485_3_WaitTime 6
|
|
#define RS485_4_WaitTime 6
|
|
#define LTE_7S0_Serial_WaitTime 6
|
|
#define InterCall_DEBUG_WaitTime 6
|
|
#define E28_SBUS_WaitTime 4
|
|
#define LPUART1_UART_WaitTime 6
|
|
#define RS485_1_Dispacher_Time 100
|
|
#define RS485_2_Dispacher_Time 100
|
|
#define RS485_3_Dispacher_Time 100
|
|
#define RS485_4_Dispacher_Time 100
|
|
#define LTE_7S0_Serial_Dispacher_Time 100
|
|
#define InterCall_DEBUG_Dispacher_Time 100
|
|
#define E28_SBUS_Dispacher_Time 100
|
|
#define LPUART1_UART_Dispacher_Time 100
|
|
int can1_sendListPeriod = 10;
|
|
int can1_DispacherPeriod = 4;
|
|
int can2_sendListPeriod = 10;
|
|
int can2_DispacherPeriod = 20;
|
|
|
|
int SAVE_To_CV = 0;
|
|
|
|
/* USER CODE END PD */
|
|
|
|
/* Private macro -------------------------------------------------------------*/
|
|
/* USER CODE BEGIN PM */
|
|
|
|
/* USER CODE END PM */
|
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
|
|
/* USER CODE BEGIN PV */
|
|
void protobuf_mqtt_setting_test(void);
|
|
/* USER CODE END PV */
|
|
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
void SystemClock_Config(void);
|
|
static void MPU_Config(void);
|
|
/* USER CODE BEGIN PFP */
|
|
char ReadIO[8];
|
|
|
|
/* USER CODE END PFP */
|
|
|
|
/* Private user code ---------------------------------------------------------*/
|
|
/* USER CODE BEGIN 0 */
|
|
|
|
/* USER CODE END 0 */
|
|
|
|
void GF_Robot_Init()
|
|
{
|
|
|
|
|
|
|
|
//2ms调度
|
|
GF_BSP_EEPROM_Init();
|
|
|
|
CV_GV_Init();
|
|
|
|
//初始化encoder
|
|
GF_BSP_UARTHandlers_Intialize(
|
|
RS485_1_WaitTime,
|
|
RS485_2_WaitTime,
|
|
RS485_3_WaitTime,
|
|
RS485_4_WaitTime,
|
|
LTE_7S0_Serial_WaitTime,
|
|
InterCall_DEBUG_WaitTime,
|
|
E28_SBUS_WaitTime,
|
|
LPUART1_UART_WaitTime,
|
|
RS485_1_Dispacher_Time,
|
|
RS485_2_Dispacher_Time,
|
|
RS485_3_Dispacher_Time,
|
|
RS485_4_Dispacher_Time,
|
|
LTE_7S0_Serial_Dispacher_Time,
|
|
InterCall_DEBUG_Dispacher_Time,
|
|
E28_SBUS_Dispacher_Time,
|
|
LPUART1_UART_Dispacher_Time);
|
|
|
|
|
|
|
|
is_upper_computer_take_over_control = 0;
|
|
DLT_LOG_ENABLE_LEVEL = 7; //7 send all information //0 send nothing
|
|
//dLT_Log_intialize(&InterCall_DEBUG_UART_Handler); //huart4
|
|
//dLT_Log_intialize(&RS_485_4_UART_Handler);
|
|
udp_client_init();
|
|
tcp_server_init(3490);
|
|
dLT_Log_intialize_udp_tcp();
|
|
upper_Computer_UART_Handler_intialize(&RS_485_4_UART_Handler);//send to computer to 485
|
|
TL720D_intialize(&RS_485_1_UART_Handler);
|
|
MK32_Sbus_UART_Handler_intialize(&E28_SBUS_UART_Handler); //huart5
|
|
client_setting_intialize(&LPUART1_UART_Handler);
|
|
|
|
//WH_LTE_7S0_intialize(<E_7S0_Serial_UART_Handler);//huart2
|
|
|
|
GF_BSP_FDCAN_Init();//初始化FDCAN
|
|
|
|
GF_BSP_CANHandler_Init(can1_sendListPeriod, can1_DispacherPeriod,
|
|
can2_sendListPeriod, can2_DispacherPeriod);
|
|
|
|
|
|
LS_Motor_Controller_intialize(&FD_CAN_1_Handler);
|
|
|
|
Fsm_Init();
|
|
|
|
GF_BSP_TIMER_Init(); //定时器最后启
|
|
|
|
|
|
}
|
|
/*
|
|
* */
|
|
void CV_GV_Init()
|
|
{
|
|
//Read CV
|
|
CV = GF_BSP_EEPROM_Get_CV();
|
|
CV.has_PID_high=true;
|
|
CV.has_PID_mid=true;
|
|
CV.has_PID_low=true;
|
|
|
|
SystemErrorCode = &GV.SystemErrorData.Com_Error_Code;
|
|
|
|
//电机绑定
|
|
LS_Motor[1]=&GV.LeftMotor;
|
|
LS_Motor[2]=&GV.RightMotor;
|
|
|
|
LS_Motor[1]->MotorID=1;
|
|
LS_Motor[2]->MotorID=2;
|
|
|
|
P_MK32 = &GV.P_MK32;
|
|
|
|
SP_MSP_RF_TL720D_Parameters_In = &GV.TL720DParameters;
|
|
|
|
|
|
|
|
RobotAngle = &GV.Robot_Angle;
|
|
|
|
tempature = &GV.TempatureE_2C;
|
|
|
|
GV.has_P_MK32 = true;
|
|
GV.has_LeftMotor = true;
|
|
GV.has_RightMotor = true;
|
|
GV.has_TL720DParameters = true;
|
|
GV.has_IO = true;
|
|
GV.has_SystemErrorData = true;
|
|
|
|
}
|
|
|
|
int BHBF_robot_init(void)
|
|
{
|
|
SystemTimer_Intialize();
|
|
|
|
Error_Detect_Intialzie(1000); //every 1 seconds
|
|
GF_Robot_Init();
|
|
GV.Robot_Move_Speed = 1;
|
|
return 0;
|
|
}
|
|
|
|
void BHBF_robot_task(void)
|
|
{
|
|
/* 刷新看门狗计数器（喂狗） */
|
|
//HAL_IWDG_Refresh(&hiwdg1);
|
|
|
|
if (SAVE_To_CV == 1)
|
|
{
|
|
SAVE_To_CV = 0;
|
|
//CV写入falsh�???????
|
|
GF_BSP_EEPROM_Set_CV(CV);
|
|
CV = GF_BSP_EEPROM_Get_CV();
|
|
//FS_SetZero();
|
|
}
|
|
}
|