118 lines
3.5 KiB
C
118 lines
3.5 KiB
C
/*
|
||
Task8 Task
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include "bsp/can.h"
|
||
#include "device/motor_rm.h"
|
||
#include "component/pid.h"
|
||
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* USER STRUCT BEGIN */
|
||
MOTOR_RM_t motor_6020;
|
||
MOTOR_RM_t motor_3508;
|
||
MOTOR_RM_Param_t motor_6020_param = {
|
||
.can = BSP_CAN_1,
|
||
.id = 0x205,
|
||
.module = MOTOR_GM6020,
|
||
.reverse = false,
|
||
.gear = false,
|
||
};
|
||
MOTOR_RM_Param_t motor_3508_param = {
|
||
.can = BSP_CAN_1,
|
||
.id = 0x201,
|
||
.module = MOTOR_M3508,
|
||
.reverse = false,
|
||
.gear = false,
|
||
};
|
||
|
||
KPID_t motor_6020_speed;
|
||
KPID_t motor_3508_angle;
|
||
KPID_t motor_3508_omega;
|
||
|
||
KPID_Params_t motor_6020_speed_param = {
|
||
.k = 0.1f,
|
||
.p = 0.2f,
|
||
.i = 0.1f,
|
||
.d = 0.0f,
|
||
.i_limit = 1.0f,
|
||
.out_limit = 1.0f,
|
||
.d_cutoff_freq = 30.0f,
|
||
};
|
||
KPID_Params_t motor_3508_angle_param = {
|
||
.k = 1.0f,
|
||
.p = 5.0f,
|
||
.i = 0.1f,
|
||
.d = 0.0f,
|
||
.i_limit = 1.0f,
|
||
.out_limit = 1.0f,
|
||
.d_cutoff_freq = 30.0f,
|
||
};
|
||
KPID_Params_t motor_3508_omega_param = {
|
||
.k = 1.0f,
|
||
.p = 5.0f,
|
||
.i = 0.1f,
|
||
.d = 0.0f,
|
||
.i_limit = 1.0f,
|
||
.out_limit = 1.0f,
|
||
.d_cutoff_freq = 30.0f,
|
||
};
|
||
|
||
float motor_3508_target_angle = 5*M_2PI;
|
||
float motor_6020_target_speed = 33.0f;
|
||
/* USER STRUCT END */
|
||
|
||
/* Private function --------------------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
void Task_Task8(void *argument) {
|
||
(void)argument; /* 未使用argument，消除警告 */
|
||
|
||
|
||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK8_FREQ;
|
||
|
||
osDelay(TASK8_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||
|
||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||
/* USER CODE INIT BEGIN */
|
||
BSP_CAN_Init();
|
||
MOTOR_RM_Register( &motor_6020_param);
|
||
MOTOR_RM_Register(&motor_3508_param);
|
||
|
||
PID_Init(&motor_6020_speed, KPID_MODE_CALC_D, TASK8_FREQ, &motor_6020_speed_param);
|
||
PID_Init(&motor_3508_angle, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_angle_param);
|
||
PID_Init(&motor_3508_omega, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_omega_param);
|
||
/* USER CODE INIT END */
|
||
|
||
while (1) {
|
||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
/* USER CODE BEGIN */
|
||
MOTOR_RM_UpdateAll();
|
||
|
||
motor_6020.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_6020_param)->motor)) );
|
||
motor_3508.feedback.rotor_abs_angle= MOTOR_RM_GetMotor(&motor_3508_param)->gearbox_total_angle;
|
||
motor_3508.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_3508_param)->motor)) );
|
||
|
||
|
||
float motor_6020out,motor_3508_ommega,motor_3508_out;
|
||
|
||
motor_6020out = PID_Calc(&motor_6020_speed, motor_6020_target_speed, motor_6020.feedback.rotor_speed, 0.0f,0.0f);
|
||
|
||
// motor_3508_ommega = PID_Calc(&motor_3508_omega, motor_3508_target_angle, motor_3508.feedback.rotor_abs_angle, 0.0f,0.0f);
|
||
// motor_3508_out = PID_Calc(&motor_3508_angle, motor_3508_ommega, motor_3508.feedback.rotor_speed, 0.0f,0.0f);
|
||
|
||
MOTOR_RM_SetOutput(&motor_6020_param, motor_6020out);
|
||
MOTOR_RM_Ctrl(&motor_6020_param);
|
||
/* USER CODE END */
|
||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
|
||
} |