84 lines
2.3 KiB
C
84 lines
2.3 KiB
C
/**
|
|
******************************************************************************
|
|
* @file : Control_Task.c
|
|
* @brief : Control task
|
|
* @author : GrassFan Wang
|
|
* @date : 2025/01/22
|
|
* @version : v1.1
|
|
******************************************************************************
|
|
* @attention : None
|
|
******************************************************************************
|
|
*/
|
|
/* USER CODE END Header */
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "Control_Task.h"
|
|
#include "cmsis_os.h"
|
|
#include "Control_Task.h"
|
|
#include "bsp_uart.h"
|
|
#include "Remote_Control.h"
|
|
#include "PID.h"
|
|
#include "Motor.h"
|
|
|
|
static void Control_Init(Control_Info_Typedef *Control_Info);
|
|
static void Control_Measure_Update(Control_Info_Typedef *Control_Info);
|
|
static void Control_Target_Update(Control_Info_Typedef *Control_Info);
|
|
static void Control_Info_Update(Control_Info_Typedef *Control_Info);
|
|
|
|
Control_Info_Typedef Control_Info;
|
|
// KP KI KD Alpha Deadband I_MAX Output_MAX
|
|
static float Chassis_PID_Param[7] = {13.f,0.1f,0.f,0.f, 0.f, 5000.f, 12000.f};
|
|
|
|
PID_Info_TypeDef Chassis_PID;
|
|
|
|
void Control_Task(void const * argument)
|
|
{
|
|
/* USER CODE BEGIN Control_Task */
|
|
TickType_t Control_Task_SysTick = 0;
|
|
|
|
Control_Init(&Control_Info);
|
|
/* Infinite loop */
|
|
for(;;)
|
|
{
|
|
Control_Task_SysTick = osKernelSysTick();
|
|
|
|
|
|
Control_Measure_Update(&Control_Info);
|
|
Control_Target_Update(&Control_Info);
|
|
Control_Info_Update(&Control_Info);
|
|
USART_Vofa_Justfloat_Transmit(Control_Info.Measure.Chassis_Velocity,0.f,0.f);
|
|
|
|
osDelay(1);
|
|
}
|
|
}
|
|
/* USER CODE END Control_Task */
|
|
|
|
static void Control_Init(Control_Info_Typedef *Control_Info){
|
|
|
|
PID_Init(&Chassis_PID,PID_POSITION,Chassis_PID_Param);
|
|
|
|
}
|
|
|
|
static void Control_Measure_Update(Control_Info_Typedef *Control_Info){
|
|
|
|
Control_Info->Measure.Chassis_Velocity = Chassis_Motor[0].Data.Velocity;
|
|
|
|
}
|
|
|
|
static void Control_Target_Update(Control_Info_Typedef *Control_Info){
|
|
|
|
Control_Info->Target.Chassis_Velocity = remote_ctrl.rc.ch[3] * 5.f;
|
|
|
|
|
|
}
|
|
|
|
static void Control_Info_Update(Control_Info_Typedef *Control_Info){
|
|
|
|
PID_Calculate(&Chassis_PID, Control_Info->Target.Chassis_Velocity, Control_Info->Measure.Chassis_Velocity);
|
|
|
|
Control_Info->SendValue[0] = (int16_t)(Chassis_PID.Output);
|
|
|
|
}
|
|
|
|
|
|
|