diff --git a/User/device/remote_control.c b/User/device/remote_control.c index bc37c0b..791719b 100644 --- a/User/device/remote_control.c +++ b/User/device/remote_control.c @@ -10,7 +10,7 @@ #include "bsp/calc_lib.h" #define R12DS -//#define DR16 +// #define DR16 /* 两个遥控器都是 x @@ -85,25 +85,25 @@ static bool DR16_DataCorrupted(const DR16_t *dr16) { if ((dr16->data.ch_r_x < REMOTE_CH_VALUE_MIN) || (dr16->data.ch_r_x > REMOTE_CH_VALUE_MAX)) - return true; + return false; if ((dr16->data.ch_r_y < REMOTE_CH_VALUE_MIN) || (dr16->data.ch_r_y > REMOTE_CH_VALUE_MAX)) - return true; + return false; if ((dr16->data.ch_l_x < REMOTE_CH_VALUE_MIN) || (dr16->data.ch_l_x > REMOTE_CH_VALUE_MAX)) - return true; + return false; if ((dr16->data.ch_l_y < REMOTE_CH_VALUE_MIN) || (dr16->data.ch_l_y > REMOTE_CH_VALUE_MAX)) - return true; + return false; - if (dr16->data.sw_l == 0) return true; + if (dr16->data.sw_l == 0) return false; - if (dr16->data.sw_r == 0) return true; + if (dr16->data.sw_r == 0) return false; - return false; + return true; } int8_t DR16_ParseRaw(DR16_t *dr16){ diff --git a/User/module/chassis.c b/User/module/chassis.c index b500c7f..03cd376 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -83,7 +83,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now) PID_Reset(&c->pid.chassis_6020anglePid[i]); PID_Reset(&c->pid.chassis_3508DAOHANG_pid[i]); } - + PID_Reset(&c->pid.daohang_chassis_follow_gimbal_pid); c->mode = mode; return CHASSIS_OK; } @@ -148,6 +148,7 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq) PID_Init(&c->pid.chassis_3508DAOHANG_pid[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508DAOHANG_param); } PID_Init(&c->pid.chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->chassis_follow_gimbalPID); + PID_Init(&c->pid.daohang_chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->daohang_chassis_follow_gimbalPID); LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy @@ -411,7 +412,7 @@ case CHASSIS_MODE_DAOHANG: // 导航模式 c->move_vec.Vx = -c_cmd->Vx/1; c->move_vec.Vy = c_cmd->Vy/1; - c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);; + c->move_vec.Vw =PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);; break; default: @@ -450,6 +451,7 @@ case CHASSIS_MODE_DAOHANG: c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt); c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]); } + break; case CHASSIS_MODE_DAOHANG: for(int i=0;i<4;i++) @@ -469,6 +471,8 @@ for(int i=0;i<4;i++) c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt); c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]); } +break; + } return CHASSIS_OK; } diff --git a/User/module/chassis.h b/User/module/chassis.h index e46a923..c1a27d7 100644 --- a/User/module/chassis.h +++ b/User/module/chassis.h @@ -59,7 +59,7 @@ extern "C" KPID_Params_t M3508DAOHANG_param; KPID_Params_t chassis_follow_gimbalPID; - + KPID_Params_t daohang_chassis_follow_gimbalPID; MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机 MOTOR_RM_Param_t motor_6020_param[4]; // 四个6020电机 @@ -132,7 +132,7 @@ extern "C" KPID_t chassis_3508VPID[4]; KPID_t chassis_3508DAOHANG_pid[4]; - + KPID_t daohang_chassis_follow_gimbal_pid; KPID_t chassis_follow_gimbal_pid; } pid; diff --git a/User/module/cmd.c b/User/module/cmd.c index 5bd3d36..8d7c95f 100644 --- a/User/module/cmd.c +++ b/User/module/cmd.c @@ -155,7 +155,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd // c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL; break; case CMD_SW_DOWN: - g_cmd->ctrl_mode = GIMBAL_MODE_AI; + g_cmd->ctrl_mode = GIMBAL_MODE_DAOHANG; s_cmd->control_mode = SHOOT_AI; c_cmd->mode = CHASSIS_MODE_DAOHANG; break; diff --git a/User/module/cmd.h b/User/module/cmd.h index ca1f883..8c7bb41 100644 --- a/User/module/cmd.h +++ b/User/module/cmd.h @@ -106,11 +106,13 @@ typedef struct GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ + GIMBAL_MODE_DAOHANG, /* 导航模式，云台根据导航数据进行移动 */ } Gimbal_Mode_t; typedef enum { GIMBAL_MODE_REMOTE, GIMBAL_MODE_AI, + }GIMBAL_Ctrl_mode_t; typedef struct { diff --git a/User/module/config.c b/User/module/config.c index ed2e45c..57a75fd 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -57,7 +57,7 @@ static const Config_Param_t config = { // .d_cutoff_freq= -1.0f, // .range=-1.0f .k = 1.0f, - .p = 4.0f, + .p = 3.0f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -68,7 +68,7 @@ static const Config_Param_t config = { .M3508DAOHANG_param={ .k = 1.0f, - .p = 2.0f, + .p = 1.0f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -76,14 +76,24 @@ static const Config_Param_t config = { .d_cutoff_freq = -1.0f, .range = -1.0f }, - - .chassis_follow_gimbalPID = { - .k = 0.7f, - .p = 0.5f, - .i = 0.1f, + .daohang_chassis_follow_gimbalPID={ + .k = 1.0f, + .p = 2.5f, + .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, - .out_limit = 0.5f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI + }, + + .chassis_follow_gimbalPID = { + .k = 0.5f, + .p = 0.5f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .range = M_2PI }, @@ -235,6 +245,27 @@ static const Config_Param_t config = { .range = -1.0f }, +.pid.daohang_6020_motor_angle={ + .k = 1.0f, + .p = 2.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = 20.0f, + .range = M_2PI + }, +.pid.daohang_4310_motor_angle={ + .k = 2.0f, + .p = 5.0f, + .i = 0.0f, + .d = 1.0f, + .i_limit = 0.0f, + .out_limit = 2.0f, + .d_cutoff_freq = 20.0f, + .range = M_2PI + }, + .mech_zero = { .yaw_6020 = 1.31f,/*1.31*/ .yaw_4310 = 2.06f, /*大yaw零点*/ diff --git a/User/module/gimbal.c b/User/module/gimbal.c index b625241..4e6ee7a 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -74,6 +74,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { PID_Reset(&g->pid.pitch_4310_omega); PID_Reset(&g->pid.yaw_6020_angle); PID_Reset(&g->pid.yaw_6020_omega); + PID_Reset(&g->pid.daohang_6020_angle); + PID_Reset(&g->pid.daohang_4310_angle); LowPassFilter2p_Reset(&g->filter_out.yaw_4310,0.0f); LowPassFilter2p_Reset(&g->filter_out.pitch_4310,0.0f); LowPassFilter2p_Reset(&g->filter_out.yaw_6020,0.0f); @@ -119,6 +121,9 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) /*4310 pitch pid初始化 单环*/ PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle ); PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega ); +/*导航PID*/ + PID_Init(&g->pid.daohang_6020_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_6020_motor_angle ); + PID_Init(&g->pid.daohang_4310_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_4310_motor_angle ); /*输出滤波器初始化*/ LowPassFilter2p_Init(&g->filter_out.yaw_6020, target_freq, g->param->low_pass_cutoff_freq.out); LowPassFilter2p_Init(&g->filter_out.gyro_yaw, target_freq, g->param->low_pass_cutoff_freq.gyro); @@ -364,6 +369,16 @@ g->setpoint.pit_accl = g_cmd->pit_accl; g->out.yaw_6020 = 0.0f; break; + case GIMBAL_MODE_DAOHANG: + /*导航模式，NUC直接给出目标角度，云台控制环跟随即可*/ + g->setpoint.eulr.yaw = g->param->mech_zero.yaw_6020+g->param->travel.yaw_6020*0.5f; // 这里可以直接设置为NUC传来的目标角度 + g->setpoint.yaw_4310 = 1.44215584f; + + g->out.yaw_6020 = PID_Calc(&g->pid.daohang_6020_angle,g->setpoint.eulr.yaw, + g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,0.0f,g->dt); + g->out.yaw_4310 = PID_Calc(&g->pid.daohang_4310_angle,g->setpoint.yaw_4310, + g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt); + /* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/ // g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020); g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020); diff --git a/User/module/gimbal.h b/User/module/gimbal.h index 5869f76..87a5516 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -42,6 +42,9 @@ extern "C" KPID_Params_t yaw_4310_motor_angle; /* yaw轴4310角位置环PID参数 */ KPID_Params_t pitch_4310_motor_omega; /* pitch轴角速度环PID参数 */ KPID_Params_t pitch_4310_motor_angle; /* pitch轴角位置环PID参数 */ + + KPID_Params_t daohang_6020_motor_angle; /* 导航模式小yaw角位置环PID参数 */ + KPID_Params_t daohang_4310_motor_angle; /* 导航模式大yaw角位置环PID参数 */ } pid; /* 低通滤波器截止频率 */ @@ -134,6 +137,9 @@ extern "C" KPID_t yaw_4310_omega; /* yaw轴4310角速度环PID */ KPID_t pitch_4310_angle; /* pitch轴4310角位置环PID */ KPID_t pitch_4310_omega; /* pitch轴4310角速度环PID */ + + KPID_t daohang_6020_angle; /* 导航模式小yaw角位置环PID */ + KPID_t daohang_4310_angle; /* 导航模式大yaw角位置环PID */ } pid; struct diff --git a/User/task/gimbal.c b/User/task/gimbal.c index 0d3db23..4b0a3db 100644 --- a/User/task/gimbal.c +++ b/User/task/gimbal.c @@ -66,7 +66,8 @@ osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0); if(ai_gimbal_result_cmd.mode==0){ cmd_gimbal.set_pitch =gimbal_imu.eulr.pit; - cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw; + cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw; + } if(ai_gimbal_result_cmd.mode==1) { @@ -76,6 +77,7 @@ if(ai_gimbal_result_cmd.mode==0){ cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw; cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit; cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit; + } if(ai_gimbal_result_cmd.mode==2){ cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit; @@ -84,7 +86,10 @@ if(ai_gimbal_result_cmd.mode==0){ cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw; cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit; cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit; + } + + Gimbal_UpdateFeedback(&gimbal); osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);