100 lines
3.0 KiB
C
100 lines
3.0 KiB
C
/*
|
||
ctrl_gimbal Task
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "device/motor_rm.h"
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include "bsp/can.h"
|
||
#include "device/motor_dm.h"
|
||
#include "device/motor_rm.h"
|
||
#include "device/motor.h"
|
||
#include "component/pid.h"
|
||
#include <stdbool.h>
|
||
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* USER STRUCT BEGIN */
|
||
MOTOR_DM_Param_t dm_4310_param = {
|
||
.can = BSP_CAN_1,
|
||
.master_id = 0x11, // 主站ID
|
||
.can_id = 0x01, // 反馈ID 10进制是85
|
||
.module = MOTOR_DM_J4310,
|
||
.reverse = false,
|
||
};
|
||
|
||
// MOTOR_RM_Param_t m2006 = {
|
||
// .can = BSP_CAN_1,
|
||
// .id = 0x201,
|
||
// .module = MOTOR_M2006,
|
||
// .reverse = false,
|
||
// .gear = true,
|
||
// };
|
||
|
||
MOTOR_MIT_Output_t gimbal_output = {
|
||
.angle = 0.0f,
|
||
.velocity = 0.0f,
|
||
.torque = 0.0f,
|
||
.kp = 0.0f,
|
||
.kd = 0.0f,
|
||
};
|
||
MOTOR_DM_t *dm_4310_motor = NULL;
|
||
|
||
// MOTOR_Feedback_t m2006_fb;
|
||
MOTOR_Feedback_t dm_4310_fb;
|
||
|
||
// KPID_Params_t gimbal_angle_pid = {
|
||
// .leg_theta={
|
||
// .k=1.0f,
|
||
// .p=5.0f, /* 摆角比例系数 */
|
||
// .i=0.0f, /* 摆角积分系数 */
|
||
// .d=0.0f, /* 摆角微分系数 */
|
||
// .i_limit=0.0f, /* 积分限幅 */
|
||
// .out_limit=0.05f, /* 输出限幅，腿长差值限制 */
|
||
// .d_cutoff_freq=30.0f, /* 微分截止频率 */
|
||
// .range=-1.0f, /* 不使用循环误差处理 */
|
||
// };
|
||
/* USER STRUCT END */
|
||
|
||
/* Private function --------------------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
void Task_ctrl_gimbal(void *argument) {
|
||
(void)argument; /* 未使用argument，消除警告 */
|
||
|
||
|
||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_GIMBAL_FREQ;
|
||
|
||
osDelay(CTRL_GIMBAL_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||
|
||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||
/* USER CODE INIT BEGIN */
|
||
BSP_CAN_Init();
|
||
// MOTOR_DM_Register(&dm_4310_param);
|
||
|
||
while (1) {
|
||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
/* USER CODE BEGIN */
|
||
// MOTOR_DM_Update(&dm_4310_param);
|
||
// MOTOR_DM_t *dm_4310_motor = MOTOR_DM_GetMotor(&dm_4310_param);
|
||
// if (dm_4310_motor) {
|
||
// dm_4310_fb = dm_4310_motor->motor.feedback;
|
||
// }
|
||
// if (dm_4310_motor->motor.header.online == false) {
|
||
// MOTOR_DM_Enable(&dm_4310_param); // 使能电机
|
||
// } else {
|
||
// MOTOR_DM_MITCtrl(&dm_4310_param, &gimbal_output);
|
||
// }
|
||
|
||
|
||
/* USER CODE END */
|
||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
|
||
} |