缝缝补补也算是有了双轴限位不过还有bug
This commit is contained in:
parent
6c7e02f5c7
commit
fa5dae4e9f
@ -412,6 +412,16 @@
|
|||||||
<WinNumber>1</WinNumber>
|
<WinNumber>1</WinNumber>
|
||||||
<ItemText>cmd_host</ItemText>
|
<ItemText>cmd_host</ItemText>
|
||||||
</Ww>
|
</Ww>
|
||||||
|
<Ww>
|
||||||
|
<count>11</count>
|
||||||
|
<WinNumber>1</WinNumber>
|
||||||
|
<ItemText>gimbal_ahrs</ItemText>
|
||||||
|
</Ww>
|
||||||
|
<Ww>
|
||||||
|
<count>12</count>
|
||||||
|
<WinNumber>1</WinNumber>
|
||||||
|
<ItemText>eulr_to_send</ItemText>
|
||||||
|
</Ww>
|
||||||
</WatchWindow1>
|
</WatchWindow1>
|
||||||
<Tracepoint>
|
<Tracepoint>
|
||||||
<THDelay>0</THDelay>
|
<THDelay>0</THDelay>
|
||||||
|
Binary file not shown.
18617
MDK-ARM/DevC/DevC.hex
18617
MDK-ARM/DevC/DevC.hex
File diff suppressed because it is too large
Load Diff
@ -225,7 +225,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case CMD_SW_MID:
|
case CMD_SW_MID:
|
||||||
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
|
||||||
cmd->shoot.fire = false;
|
cmd->shoot.fire = false;
|
||||||
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
cmd->shoot.mode = SHOOT_MODE_LOADED;
|
||||||
break;
|
break;
|
||||||
|
@ -42,8 +42,8 @@ typedef enum {
|
|||||||
typedef enum {
|
typedef enum {
|
||||||
GIMBAL_MODE_RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */
|
GIMBAL_MODE_RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */
|
||||||
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */
|
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */
|
||||||
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制,控制相对于底盘的姿态 */
|
|
||||||
GIMBAL_MODE_SEARCH, /* 搜索模式,通过云台旋转搜索目标 */
|
GIMBAL_MODE_SEARCH, /* 搜索模式,通过云台旋转搜索目标 */
|
||||||
|
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制,控制相对于底盘的姿态 */
|
||||||
} CMD_GimbalMode_t;
|
} CMD_GimbalMode_t;
|
||||||
|
|
||||||
/* 射击运行模式 */
|
/* 射击运行模式 */
|
||||||
|
@ -101,9 +101,9 @@ error:
|
|||||||
|
|
||||||
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
|
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||||
cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
|
cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
|
||||||
// cmd_host->gimbal_delta.yaw = cmd_host->gimbal_delta.yaw - 0.00015;
|
cmd_host->gimbal_delta.yaw = cmd_host->gimbal_delta.yaw - 0.00025;
|
||||||
cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
|
cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
|
||||||
cmd_host->gimbal_delta.pit = cmd_host->gimbal_delta.pit + 0.0002;
|
cmd_host->gimbal_delta.pit = cmd_host->gimbal_delta.pit + 0.0005;
|
||||||
Clip(&(cmd_host->gimbal_delta.yaw), -AI_HOST_MAX_CONTROL_VALUE,
|
Clip(&(cmd_host->gimbal_delta.yaw), -AI_HOST_MAX_CONTROL_VALUE,
|
||||||
AI_HOST_MAX_CONTROL_VALUE);
|
AI_HOST_MAX_CONTROL_VALUE);
|
||||||
Clip(&(cmd_host->gimbal_delta.pit), -AI_HOST_MAX_CONTROL_VALUE,
|
Clip(&(cmd_host->gimbal_delta.pit), -AI_HOST_MAX_CONTROL_VALUE,
|
||||||
|
@ -200,7 +200,7 @@ typedef union {
|
|||||||
} CAN_GimbalMotor_t;
|
} CAN_GimbalMotor_t;
|
||||||
|
|
||||||
typedef union {
|
typedef union {
|
||||||
CAN_MotorFeedback_t as_array[3];
|
CAN_MotorFeedback_t as_array[4];
|
||||||
struct {
|
struct {
|
||||||
CAN_MotorFeedback_t fric1;
|
CAN_MotorFeedback_t fric1;
|
||||||
CAN_MotorFeedback_t fric2;
|
CAN_MotorFeedback_t fric2;
|
||||||
|
@ -138,7 +138,12 @@ static const Config_RobotParam_t param_default = {
|
|||||||
},
|
},
|
||||||
}, /* pid */
|
}, /* pid */
|
||||||
|
|
||||||
.pitch_travel_rad = 1.05f,
|
.limit_and_imu = {
|
||||||
|
.yaw_travel_rad = -1.0f,
|
||||||
|
.pitch_travel_rad = 2.0f,
|
||||||
|
.yaw_imu = false,
|
||||||
|
.pit_imu = false,
|
||||||
|
},
|
||||||
|
|
||||||
.low_pass_cutoff_freq = {
|
.low_pass_cutoff_freq = {
|
||||||
.out = -1.0f,
|
.out = -1.0f,
|
||||||
@ -150,7 +155,6 @@ static const Config_RobotParam_t param_default = {
|
|||||||
.pit = true,
|
.pit = true,
|
||||||
},
|
},
|
||||||
|
|
||||||
.pit_ctrl_reverse = false,
|
|
||||||
}, /* gimbal */
|
}, /* gimbal */
|
||||||
|
|
||||||
.shoot = { /* 射击模块参数 */
|
.shoot = { /* 射击模块参数 */
|
||||||
@ -285,7 +289,12 @@ static const Config_RobotParam_t param_hero = {
|
|||||||
},
|
},
|
||||||
}, /* pid */
|
}, /* pid */
|
||||||
|
|
||||||
.pitch_travel_rad = 1.07685447f,
|
.limit_and_imu = {
|
||||||
|
.yaw_travel_rad = -1.0f,
|
||||||
|
.pitch_travel_rad = 1.0f,
|
||||||
|
.yaw_imu = true,
|
||||||
|
.pit_imu = true,
|
||||||
|
},
|
||||||
|
|
||||||
.low_pass_cutoff_freq = {
|
.low_pass_cutoff_freq = {
|
||||||
.out = -1.0f,
|
.out = -1.0f,
|
||||||
@ -297,7 +306,6 @@ static const Config_RobotParam_t param_hero = {
|
|||||||
.pit = true,
|
.pit = true,
|
||||||
},
|
},
|
||||||
|
|
||||||
.pit_ctrl_reverse = false,
|
|
||||||
}, /* gimbal */
|
}, /* gimbal */
|
||||||
|
|
||||||
.shoot = { /* 射击模块参数 */
|
.shoot = { /* 射击模块参数 */
|
||||||
@ -434,25 +442,6 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
|||||||
.d_cutoff_freq = -1.0f,
|
.d_cutoff_freq = -1.0f,
|
||||||
.range = M_2PI,
|
.range = M_2PI,
|
||||||
}, {
|
}, {
|
||||||
// /* GIMBAL_PID_PIT_OMEGA_IDX */
|
|
||||||
// .k = 0.35f,
|
|
||||||
// .p = 1.0f,
|
|
||||||
// .i = 0.f,
|
|
||||||
// .d = 0.003f,
|
|
||||||
// .i_limit = 1.0f,
|
|
||||||
// .out_limit = 1.0f,
|
|
||||||
// .d_cutoff_freq = -1.0f,
|
|
||||||
// .range = -1.0f,
|
|
||||||
// }, {
|
|
||||||
// /* GIMBAL_PID_PIT_ANGLE_IDX */
|
|
||||||
// .k = 15.0f,
|
|
||||||
// .p = 1.0f,
|
|
||||||
// .i = 0.0f,
|
|
||||||
// .d = 0.0f,
|
|
||||||
// .i_limit = 0.0f,
|
|
||||||
// .out_limit = 10.0f,
|
|
||||||
// .d_cutoff_freq = -1.0f,
|
|
||||||
// .range = M_2PI,
|
|
||||||
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
/* GIMBAL_PID_PIT_OMEGA_IDX */
|
||||||
.k = 0.25f,
|
.k = 0.25f,
|
||||||
.p = 1.0f,
|
.p = 1.0f,
|
||||||
@ -475,7 +464,12 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
|||||||
},
|
},
|
||||||
}, /* pid */
|
}, /* pid */
|
||||||
|
|
||||||
.pitch_travel_rad = 0.85f,
|
.limit_and_imu = {
|
||||||
|
.yaw_travel_rad = -1.0f,
|
||||||
|
.pitch_travel_rad = 1.0f,
|
||||||
|
.yaw_imu = true,
|
||||||
|
.pit_imu = true,
|
||||||
|
},
|
||||||
|
|
||||||
.low_pass_cutoff_freq = {
|
.low_pass_cutoff_freq = {
|
||||||
.out = -1.0f,
|
.out = -1.0f,
|
||||||
@ -487,7 +481,6 @@ static const Config_RobotParam_t param_sentry_chassis = {
|
|||||||
.pit = true,
|
.pit = true,
|
||||||
},
|
},
|
||||||
|
|
||||||
.pit_ctrl_reverse = true,
|
|
||||||
}, /* gimbal */
|
}, /* gimbal */
|
||||||
|
|
||||||
.shoot = { /* 射击模块参数 */
|
.shoot = { /* 射击模块参数 */
|
||||||
@ -665,8 +658,13 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
|||||||
},
|
},
|
||||||
}, /* pid */
|
}, /* pid */
|
||||||
|
|
||||||
.pitch_travel_rad = 0.85f,
|
.limit_and_imu = {
|
||||||
|
.yaw_travel_rad = -1.0f,
|
||||||
|
.pitch_travel_rad = 1.0f,
|
||||||
|
.yaw_imu = true,
|
||||||
|
.pit_imu = true,
|
||||||
|
},
|
||||||
|
|
||||||
.low_pass_cutoff_freq = {
|
.low_pass_cutoff_freq = {
|
||||||
.out = -1.0f,
|
.out = -1.0f,
|
||||||
.gyro = 1000.0f,
|
.gyro = 1000.0f,
|
||||||
@ -677,7 +675,6 @@ static const Config_RobotParam_t param_sentry_gimbal = {
|
|||||||
.pit = true,
|
.pit = true,
|
||||||
},
|
},
|
||||||
|
|
||||||
.pit_ctrl_reverse = true,
|
|
||||||
}, /* gimbal */
|
}, /* gimbal */
|
||||||
|
|
||||||
.shoot = { /* 射击模块参数 */
|
.shoot = { /* 射击模块参数 */
|
||||||
|
@ -47,8 +47,8 @@ typedef struct {
|
|||||||
} cali; /* 校准 */
|
} cali; /* 校准 */
|
||||||
|
|
||||||
AHRS_Eulr_t mech_zero; /* 机械零点 */
|
AHRS_Eulr_t mech_zero; /* 机械零点 */
|
||||||
float gimbal_limit; /* 云台pitch轴软件限位最高点 */
|
float gimbal_pitch_limit; /* 云台pitch轴软件限位最高点 */
|
||||||
|
float gimbal_yaw_limit; /* 云台yaw轴软件限位最高点 */
|
||||||
} Config_t;
|
} Config_t;
|
||||||
|
|
||||||
/* 机器人参数和对应字符串的映射 */
|
/* 机器人参数和对应字符串的映射 */
|
||||||
|
@ -9,9 +9,6 @@
|
|||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
/* Private define ----------------------------------------------------------- */
|
/* Private define ----------------------------------------------------------- */
|
||||||
#define YAW_DELTA_MAX (5.7f + M_PI)
|
|
||||||
#define YAW_DELTA_MIN (1.8f + M_PI)
|
|
||||||
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
/* Private macro ------------------------------------------------------------ */
|
||||||
/* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
/* Private function -------------------------------------------------------- */
|
/* Private function -------------------------------------------------------- */
|
||||||
@ -28,6 +25,10 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
|
|||||||
if (g == NULL) return -1;
|
if (g == NULL) return -1;
|
||||||
if (mode == g->mode) return GIMBAL_OK;
|
if (mode == g->mode) return GIMBAL_OK;
|
||||||
|
|
||||||
|
/* 获取当前反馈值,防止位置突变 */
|
||||||
|
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||||
|
g->setpoint.eulr.pit = g->feedback.eulr.encoder.pit;
|
||||||
|
|
||||||
/* 切换模式后重置PID和滤波器 */
|
/* 切换模式后重置PID和滤波器 */
|
||||||
for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) {
|
for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) {
|
||||||
PID_Reset(g->pid + i);
|
PID_Reset(g->pid + i);
|
||||||
@ -39,7 +40,11 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
|
|||||||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||||||
if (g->mode == GIMBAL_MODE_RELAX) {
|
if (g->mode == GIMBAL_MODE_RELAX) {
|
||||||
if (mode == GIMBAL_MODE_ABSOLUTE) {
|
if (mode == GIMBAL_MODE_ABSOLUTE) {
|
||||||
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
|
if (g->param->limit_and_imu.yaw_imu) {
|
||||||
|
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
|
||||||
|
} else {
|
||||||
|
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||||
|
}
|
||||||
} else if (mode == GIMBAL_MODE_RELATIVE) {
|
} else if (mode == GIMBAL_MODE_RELATIVE) {
|
||||||
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
|
||||||
} else if (mode == GIMBAL_MODE_SEARCH) {
|
} else if (mode == GIMBAL_MODE_SEARCH) {
|
||||||
@ -71,9 +76,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit_max,
|
|||||||
|
|
||||||
/* 设置软件限位 */
|
/* 设置软件限位 */
|
||||||
if (g->param->reverse.pit) CircleReverse(&limit_max);
|
if (g->param->reverse.pit) CircleReverse(&limit_max);
|
||||||
g->limit.min = g->limit.max = limit_max;
|
g->limit.pit_min = g->limit.pit_max = limit_max;
|
||||||
CircleAdd(&(g->limit.min), -g->param->pitch_travel_rad, M_2PI);
|
CircleAdd(&(g->limit.pit_min), -g->param->limit_and_imu.pitch_travel_rad, M_2PI);
|
||||||
|
|
||||||
|
if (g->param->reverse.yaw) CircleReverse(&limit_max);
|
||||||
|
g->limit.yaw_min = g->limit.yaw_max = limit_max;
|
||||||
|
CircleAdd(&(g->limit.yaw_min), -g->param->limit_and_imu.yaw_travel_rad, M_2PI);
|
||||||
/* 初始化云台电机控制PID和LPF */
|
/* 初始化云台电机控制PID和LPF */
|
||||||
PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
|
PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
|
||||||
&(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX]));
|
&(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX]));
|
||||||
@ -135,50 +143,80 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
|||||||
Gimbal_SetMode(g, g_cmd->mode);
|
Gimbal_SetMode(g, g_cmd->mode);
|
||||||
|
|
||||||
/* yaw坐标正方向与遥控器操作逻辑相反 */
|
/* yaw坐标正方向与遥控器操作逻辑相反 */
|
||||||
g_cmd->delta_eulr.pit = g_cmd->delta_eulr.pit;
|
g_cmd->delta_eulr.pit = -g_cmd->delta_eulr.pit;
|
||||||
if (g->param->pit_ctrl_reverse){
|
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
||||||
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
|
|
||||||
}else{
|
|
||||||
g_cmd->delta_eulr.yaw = g_cmd->delta_eulr.yaw;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 处理yaw控制命令 */
|
// /* 处理yaw控制命令 */
|
||||||
/* 处理yaw控制命令,添加yaw限位 */
|
// CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
||||||
const float yaw_delta_max =
|
|
||||||
CircleError(YAW_DELTA_MAX,
|
if (g->param->limit_and_imu.yaw_travel_rad > 0.0f) {
|
||||||
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
/* 处理yaw控制命令,软件限位 */
|
||||||
g->feedback.eulr.imu.yaw),
|
float delta_yaw_max;
|
||||||
M_2PI);
|
float delta_yaw_min;
|
||||||
const float yaw_delta_min =
|
|
||||||
CircleError(YAW_DELTA_MIN,
|
if (g->param->limit_and_imu.yaw_imu)
|
||||||
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
{
|
||||||
g->feedback.eulr.imu.yaw),
|
delta_yaw_max =
|
||||||
M_2PI);
|
CircleError(g->limit.yaw_max,
|
||||||
Clip(&(g_cmd->delta_eulr.yaw), yaw_delta_min, yaw_delta_max);
|
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||||
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
|
g->feedback.eulr.imu.yaw),
|
||||||
|
M_2PI);
|
||||||
|
delta_yaw_min =
|
||||||
|
CircleError(g->limit.yaw_min,
|
||||||
|
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||||
|
g->feedback.eulr.imu.yaw),
|
||||||
|
M_2PI);
|
||||||
|
}else
|
||||||
|
{
|
||||||
|
delta_yaw_max =
|
||||||
|
CircleError(g->limit.yaw_max,
|
||||||
|
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||||
|
g->feedback.eulr.encoder.yaw),
|
||||||
|
M_2PI);
|
||||||
|
delta_yaw_min =
|
||||||
|
CircleError(g->limit.yaw_min,
|
||||||
|
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
|
||||||
|
g->feedback.eulr.encoder.yaw),
|
||||||
|
M_2PI);
|
||||||
|
}
|
||||||
|
|
||||||
|
Clip(&(g_cmd->delta_eulr.yaw), delta_yaw_min, delta_yaw_max);
|
||||||
|
}else (CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI));
|
||||||
|
|
||||||
|
g->setpoint.eulr.yaw += g_cmd->delta_eulr.yaw;
|
||||||
|
if (g->param->limit_and_imu.pitch_travel_rad > 0.0f) {
|
||||||
|
/* 处理pitch控制命令,软件限位 */
|
||||||
|
float delta_pitch_max;
|
||||||
|
float delta_pitch_min;
|
||||||
|
|
||||||
|
if (g->param->limit_and_imu.pit_imu)
|
||||||
|
{
|
||||||
|
delta_pitch_max =
|
||||||
|
CircleError(g->limit.pit_max,
|
||||||
|
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||||
|
g->feedback.eulr.imu.pit),
|
||||||
|
M_2PI);
|
||||||
|
delta_pitch_min =
|
||||||
|
CircleError(g->limit.pit_min,
|
||||||
|
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||||
|
g->feedback.eulr.imu.pit),
|
||||||
|
M_2PI);
|
||||||
|
}else
|
||||||
|
{
|
||||||
|
delta_pitch_max =
|
||||||
|
CircleError(g->limit.pit_max,
|
||||||
|
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||||
|
g->feedback.eulr.encoder.pit),
|
||||||
|
M_2PI);
|
||||||
|
delta_pitch_min =
|
||||||
|
CircleError(g->limit.pit_min,
|
||||||
|
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
||||||
|
g->feedback.eulr.encoder.pit),
|
||||||
|
M_2PI);
|
||||||
|
}
|
||||||
|
Clip(&(g_cmd->delta_eulr.pit), delta_pitch_min, delta_pitch_max);
|
||||||
|
}else (CircleAdd(&(g->setpoint.eulr.pit), g_cmd->delta_eulr.pit, M_2PI));
|
||||||
|
|
||||||
/* 处理pitch控制命令,软件限位 */
|
|
||||||
// const float delta_max =
|
|
||||||
// CircleError(g->limit.max,
|
|
||||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
|
||||||
// g->feedback.eulr.imu.pit),
|
|
||||||
// M_2PI);
|
|
||||||
// const float delta_min =
|
|
||||||
// CircleError(g->limit.min,
|
|
||||||
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
|
||||||
// g->feedback.eulr.imu.pit),
|
|
||||||
// M_2PI);
|
|
||||||
const float delta_max =
|
|
||||||
CircleError(g->limit.max,
|
|
||||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
|
||||||
g->feedback.eulr.encoder.pit),
|
|
||||||
M_2PI);
|
|
||||||
const float delta_min =
|
|
||||||
CircleError(g->limit.min,
|
|
||||||
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
|
|
||||||
g->feedback.eulr.encoder.pit),
|
|
||||||
M_2PI);
|
|
||||||
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
|
|
||||||
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
|
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
|
||||||
|
|
||||||
/* 重置输入指令,防止重复处理 */
|
/* 重置输入指令,防止重复处理 */
|
||||||
@ -191,49 +229,47 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
|
|||||||
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
|
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case GIMBAL_MODE_SEARCH:
|
||||||
|
g->setpoint.eulr.pit = g->limit.pit_min + (g->limit.pit_max - g->limit.pit_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
|
||||||
|
g->setpoint.eulr.yaw = g->limit.yaw_min + (g->limit.yaw_max - g->limit.yaw_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
|
||||||
case GIMBAL_MODE_ABSOLUTE:
|
case GIMBAL_MODE_ABSOLUTE:
|
||||||
yaw_omega_set_point =
|
if (g->param->limit_and_imu.yaw_imu && g->param->limit_and_imu.pit_imu){
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
yaw_omega_set_point =
|
||||||
g->feedback.eulr.imu.yaw, 0.0f, g->dt);
|
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
||||||
g->out[GIMBAL_ACTR_YAW_IDX] =
|
g->feedback.eulr.imu.yaw, 0.0f, g->dt);
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
g->out[GIMBAL_ACTR_YAW_IDX] =
|
||||||
g->feedback.gyro.z, 0.f, g->dt);
|
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||||
|
g->feedback.gyro.z, 0.f, g->dt);
|
||||||
|
|
||||||
|
pit_omega_set_point =
|
||||||
|
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||||
|
g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
||||||
|
|
||||||
|
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||||
|
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||||
|
0.0, 0.f, g->dt);
|
||||||
|
}else{
|
||||||
|
yaw_omega_set_point =
|
||||||
|
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
||||||
|
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
|
||||||
|
g->out[GIMBAL_ACTR_YAW_IDX] =
|
||||||
|
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
||||||
|
g->feedback.gyro.z, 0.f, g->dt);
|
||||||
|
|
||||||
|
pit_omega_set_point =
|
||||||
|
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
||||||
|
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
||||||
|
|
||||||
|
g->out[GIMBAL_ACTR_PIT_IDX] =
|
||||||
|
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
||||||
|
0.0, 0.f, g->dt);
|
||||||
|
}
|
||||||
|
|
||||||
// pit_omega_set_point =
|
|
||||||
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
|
||||||
// g->feedback.eulr.imu.pit, 0.0f, g->dt);
|
|
||||||
pit_omega_set_point =
|
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
|
||||||
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
|
||||||
|
|
||||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
|
||||||
0.0, 0.f, g->dt);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case GIMBAL_MODE_RELATIVE:
|
case GIMBAL_MODE_RELATIVE:
|
||||||
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
|
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case GIMBAL_MODE_SEARCH:
|
|
||||||
// g->setpoint.eulr.yaw = YAW_DELTA_MIN + (YAW_DELTA_MAX - YAW_DELTA_MIN) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
|
|
||||||
g->setpoint.eulr.yaw = YAW_DELTA_MIN + (YAW_DELTA_MAX - YAW_DELTA_MIN) * (1.0f - sinf(now / 500.0f)) / 2.0f; // 在min和max之间摆动
|
|
||||||
g->setpoint.eulr.pit = g->limit.min + (g->limit.max - g->limit.min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
|
|
||||||
|
|
||||||
yaw_omega_set_point =
|
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
|
|
||||||
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
|
|
||||||
g->out[GIMBAL_ACTR_YAW_IDX] =
|
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
|
|
||||||
g->feedback.gyro.z, 0.f, g->dt);
|
|
||||||
|
|
||||||
pit_omega_set_point =
|
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
|
|
||||||
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
|
|
||||||
g->out[GIMBAL_ACTR_PIT_IDX] =
|
|
||||||
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
|
|
||||||
0.0, 0.f, g->dt);
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 输出滤波 */
|
/* 输出滤波 */
|
||||||
|
@ -52,22 +52,28 @@ typedef struct {
|
|||||||
float gyro; /* 陀螺仪数据 */
|
float gyro; /* 陀螺仪数据 */
|
||||||
} low_pass_cutoff_freq;
|
} low_pass_cutoff_freq;
|
||||||
|
|
||||||
float pitch_travel_rad; /* 云台pitch轴行程弧度 */
|
/* 设置云台限位和是否启用imu */
|
||||||
|
struct {
|
||||||
|
float yaw_travel_rad; /* 云台yaw轴行程弧度 */
|
||||||
|
float pitch_travel_rad; /* 云台pitch轴行程弧度 */
|
||||||
|
bool yaw_imu;
|
||||||
|
bool pit_imu;
|
||||||
|
} limit_and_imu;
|
||||||
|
|
||||||
/* 设置默认运动方向 */
|
/* 设置默认运动方向 */
|
||||||
struct {
|
struct {
|
||||||
bool yaw;
|
bool yaw;
|
||||||
bool pit;
|
bool pit;
|
||||||
} reverse;
|
} reverse;
|
||||||
|
|
||||||
bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
|
|
||||||
|
|
||||||
} Gimbal_Params_t;
|
} Gimbal_Params_t;
|
||||||
|
|
||||||
/* 软件限位 */
|
/* 软件限位 */
|
||||||
typedef struct {
|
typedef struct {
|
||||||
float max;
|
float yaw_max;
|
||||||
float min;
|
float yaw_min;
|
||||||
|
float pit_max;
|
||||||
|
float pit_min;
|
||||||
} Gimbal_Limit_t;
|
} Gimbal_Limit_t;
|
||||||
|
|
||||||
/* 云台反馈数据的结构体,包含反馈控制用的反馈数据 */
|
/* 云台反馈数据的结构体,包含反馈控制用的反馈数据 */
|
||||||
|
@ -32,10 +32,10 @@ typedef struct {
|
|||||||
|
|
||||||
static const char *const CLI_WELCOME_MESSAGE =
|
static const char *const CLI_WELCOME_MESSAGE =
|
||||||
"\r\n"
|
"\r\n"
|
||||||
// " ______ __ _______ __ \r\n"
|
" ______ __ _______ __ \r\n"
|
||||||
// " | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
" | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
|
||||||
// " | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
" | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
|
||||||
// " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
" |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
|
||||||
" -------------------------------------------------------------------\r\n"
|
" -------------------------------------------------------------------\r\n"
|
||||||
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
|
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
|
||||||
"\r\n";
|
"\r\n";
|
||||||
@ -523,7 +523,7 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
|
|||||||
switch (fsm.stage) {
|
switch (fsm.stage) {
|
||||||
case 0:
|
case 0:
|
||||||
Config_Get(&cfg);
|
Config_Get(&cfg);
|
||||||
snprintf(out_buffer, len, "(old)limit:%f\r\n", cfg.gimbal_limit);
|
snprintf(out_buffer, len, "(old)pit_limit:%f,(old)yaw_limit:%f\r\n", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit);
|
||||||
fsm.stage = 1;
|
fsm.stage = 1;
|
||||||
return pdPASS;
|
return pdPASS;
|
||||||
case 1:
|
case 1:
|
||||||
@ -538,12 +538,12 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
|
|||||||
}
|
}
|
||||||
Config_Get(&cfg);
|
Config_Get(&cfg);
|
||||||
osThreadResume(task_runtime.thread.ctrl_gimbal);
|
osThreadResume(task_runtime.thread.ctrl_gimbal);
|
||||||
cfg.gimbal_limit = can.motor.gimbal.named.pit.rotor_angle;
|
cfg.gimbal_pitch_limit = can.motor.gimbal.named.pit.rotor_angle;
|
||||||
|
cfg.gimbal_yaw_limit = can.motor.gimbal.named.yaw.rotor_angle;
|
||||||
|
|
||||||
Config_Set(&cfg);
|
Config_Set(&cfg);
|
||||||
Config_Get(&cfg);
|
Config_Get(&cfg);
|
||||||
snprintf(out_buffer, len, "(new)limit:%f\r\nDone.", cfg.gimbal_limit);
|
snprintf(out_buffer, len, "(new)limit_pitch:%f,limit_yaw%f\r\nDone.", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit);
|
||||||
|
|
||||||
fsm.stage = 2;
|
fsm.stage = 2;
|
||||||
return pdPASS;
|
return pdPASS;
|
||||||
case 2:
|
case 2:
|
||||||
@ -692,7 +692,7 @@ void Task_CLI(void *argument) {
|
|||||||
BSP_USB_Printf(CLI_WELCOME_MESSAGE);
|
BSP_USB_Printf(CLI_WELCOME_MESSAGE);
|
||||||
|
|
||||||
/* 开始运行命令行界面 */
|
/* 开始运行命令行界面 */
|
||||||
BSP_USB_Printf("rm>");
|
BSP_USB_Printf("qut-move-rm>");
|
||||||
while (1) {
|
while (1) {
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
/* 记录任务所使用的的栈空间 */
|
/* 记录任务所使用的的栈空间 */
|
||||||
@ -730,7 +730,7 @@ void Task_CLI(void *argument) {
|
|||||||
index = 0; /* 重置索引,准备接收下一段命令 */
|
index = 0; /* 重置索引,准备接收下一段命令 */
|
||||||
memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
|
memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
|
||||||
}
|
}
|
||||||
BSP_USB_Printf("rm>");
|
BSP_USB_Printf("qut-move-rm>");
|
||||||
} else if (rx_char == '\b' || rx_char == 0x7Fu) {
|
} else if (rx_char == '\b' || rx_char == 0x7Fu) {
|
||||||
/* 如果输入的是退格键则清空一位输入缓存,同时进行界限保护 */
|
/* 如果输入的是退格键则清空一位输入缓存,同时进行界限保护 */
|
||||||
if (index > 0) {
|
if (index > 0) {
|
||||||
|
@ -43,7 +43,7 @@ void Task_CtrlGimbal(void *argument) {
|
|||||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
|
||||||
/* 初始化云台 */
|
/* 初始化云台 */
|
||||||
Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal),
|
Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal),
|
||||||
task_runtime.cfg.gimbal_limit, (float)TASK_FREQ_CTRL_GIMBAL);
|
task_runtime.cfg.gimbal_pitch_limit, (float)TASK_FREQ_CTRL_GIMBAL);
|
||||||
|
|
||||||
/* 延时一段时间再开启任务 */
|
/* 延时一段时间再开启任务 */
|
||||||
osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
|
osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
|
||||||
|
Loading…
Reference in New Issue
Block a user