整理
This commit is contained in:
parent
39adf0a2b6
commit
a4a8bb6ccb
@ -135,7 +135,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
|
||||
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -208,46 +208,6 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>gimbal</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>GIMBAL_IMU_Update</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rx_msg</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>count</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rx_msg</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>can2_debug</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>fdcan2_init_debug</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rx_debug</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_for_gimbal</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -296,7 +256,7 @@
|
||||
<EnableFlashSeq>1</EnableFlashSeq>
|
||||
<EnableLog>0</EnableLog>
|
||||
<Protocol>2</Protocol>
|
||||
<DbgClock>5000000</DbgClock>
|
||||
<DbgClock>10000000</DbgClock>
|
||||
</DebugDescription>
|
||||
</TargetOption>
|
||||
</Target>
|
||||
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -39,18 +39,9 @@ extern "C" {
|
||||
typedef enum {
|
||||
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
|
||||
CHASSIS_MODE_RECOVER, /* 复位模式 */
|
||||
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
|
||||
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
|
||||
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
|
||||
CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
|
||||
} Chassis_Mode_t;
|
||||
|
||||
typedef enum {
|
||||
CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */
|
||||
CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
|
||||
CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
|
||||
CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
|
||||
} Chassis_JumpState_t;
|
||||
|
||||
typedef struct {
|
||||
Chassis_Mode_t mode; /* 底盘模式 */
|
||||
MoveVector_t move_vec; /* 底盘运动向量 */
|
||||
@ -126,13 +117,6 @@ typedef struct {
|
||||
VMC_t vmc_[2]; /* 两条腿的VMC */
|
||||
LQR_t lqr[2]; /* 两条腿的LQR控制器 */
|
||||
|
||||
int8_t state;
|
||||
uint64_t jump_time;
|
||||
|
||||
|
||||
float angle;
|
||||
float height;
|
||||
|
||||
/* 机体状态估计 */
|
||||
struct {
|
||||
float position_x; /* 机体x位置 */
|
||||
@ -151,8 +135,6 @@ typedef struct {
|
||||
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
|
||||
} yaw_control;
|
||||
|
||||
float wz_multi; /* 小陀螺模式旋转方向 */
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
AHRS_Eulr_t chassis;
|
||||
|
||||
@ -24,7 +24,7 @@ Config_RobotParam_t robot_config = {
|
||||
.gimbal_param = {
|
||||
.pid = {
|
||||
.yaw_omega = {
|
||||
.k = 1.0f,
|
||||
.k = 2.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
@ -34,8 +34,8 @@ Config_RobotParam_t robot_config = {
|
||||
.range = -1.0f,
|
||||
},
|
||||
.yaw_angle = {
|
||||
.k = 8.0f,
|
||||
.p = 3.0f,
|
||||
.k = 7.0f,
|
||||
.p = 3.5f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
@ -54,10 +54,10 @@ Config_RobotParam_t robot_config = {
|
||||
.range = -1.0f,
|
||||
},
|
||||
.pit_angle = {
|
||||
.k = 5.0f,
|
||||
.k = 2.5f,
|
||||
.p = 5.0f,
|
||||
.i = 2.5f,
|
||||
.d = 0.0f,
|
||||
.i = 0.2f,
|
||||
.d = 0.01f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
@ -66,11 +66,11 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
.mech_zero = {
|
||||
.yaw = 0.0f,
|
||||
.pit = 2.12f,
|
||||
.pit = 2.2f,
|
||||
},
|
||||
.travel = {
|
||||
.yaw = -1.0f,
|
||||
.pit = 0.9f,
|
||||
.pit = 0.85f,
|
||||
},
|
||||
.low_pass_cutoff_freq = {
|
||||
.out = -1.0f,
|
||||
@ -180,7 +180,7 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
.chassis_param = {
|
||||
.yaw={
|
||||
.k=0.0f,
|
||||
.k=1.0f,
|
||||
.p=1.0f,
|
||||
.i=0.0f,
|
||||
.d=0.3f,
|
||||
@ -297,7 +297,7 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
},
|
||||
|
||||
.mech_zero_yaw = 4.34085676f, /* 250.5度，机械零点 */
|
||||
.mech_zero_yaw = 1.78040409f, /* 机械零点 */
|
||||
|
||||
.vmc_param = {
|
||||
{ // 左腿
|
||||
@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
|
||||
.lqr_gains = {
|
||||
.k11_coeff = { -2.111003343424443e+02f, 2.534552823281283e+02f, -1.288428090302336e+02f, -4.837794661314790e-01f }, // theta
|
||||
.k12_coeff = { 9.842131763802227e+00f, -7.572331320496771e+00f, -6.747008033464407e+00f, 2.794374462854655e-02f }, // d_theta
|
||||
.k13_coeff = { -7.026854551050478e+01f, 7.582881862804882e+01f, -2.920474536554779e+01f, 1.123343465922494e+00f }, // x
|
||||
.k14_coeff = { -7.955300118741869e+01f, 8.683186601398823e+01f, -3.503519879783562e+01f, 1.204200953017506e+00f }, // d_x
|
||||
.k15_coeff = { 2.214748921482655e+01f, -5.719424594843836e+00f, -8.215498046486028e+00f, 3.357225411090161e+00f }, // phi
|
||||
.k16_coeff = { -2.588732118811617e-01f, 3.189187555191166e+00f, -3.235989186139448e+00f, 9.848256812196189e-01f }, // d_phi
|
||||
.k21_coeff = { 3.783375900877637e+02f, -3.033633007638497e+02f, 4.903946574528503e+01f, 1.550109363173939e+01f }, // theta
|
||||
.k22_coeff = { 2.878430656512739e+01f, -2.993892895824752e+01f, 1.019545567095573e+01f, 1.119137437173934e+00f }, // d_theta
|
||||
.k23_coeff = { 2.518975783541210e+01f, 2.088553619504979e+00f, -1.929185285401291e+01f, 8.558202445583612e+00f }, // x
|
||||
.k24_coeff = { 2.519335578848752e+01f, 5.878465085842500e+00f, -2.359290011046876e+01f, 1.050067818606354e+01f }, // d_x
|
||||
.k25_coeff = { 1.146427691025439e+02f, -1.303101485018965e+02f, 5.234994297700838e+01f, 1.889931627651257e+00f }, // phi
|
||||
.k26_coeff = { 3.214791991578748e+01f, -3.467989029865478e+01f, 1.318125503363052e+01f, -3.471228203459202e-01f }, // d_phi
|
||||
.k11_coeff = { -2.702074813939778e+02f, 2.894104138810802e+02f, -1.211372850409846e+02f, -6.933300731936785e-01f }, // theta
|
||||
.k12_coeff = { -3.327095836963479e+00f, 5.164647957579151e+00f, -7.601554284013210e+00f, 2.552123800155830e-01f }, // d_theta
|
||||
.k13_coeff = { -4.196624710163955e+01f, 4.127695960453792e+01f, -1.402278033829385e+01f, 7.645039434796388e-03f }, // x
|
||||
.k14_coeff = { -5.313365798300281e+01f, 5.256471525738568e+01f, -1.848736219345810e+01f, -1.124172698659643e-03f }, // d_x
|
||||
.k15_coeff = { -6.095849580858518e+01f, 7.441367039791133e+01f, -3.453552694561191e+01f, 7.000025215843615e+00f }, // phi
|
||||
.k16_coeff = { -9.662388366457952e+00f, 1.288897516182337e+01f, -6.650550674687217e+00f, 1.558651627757777e+00f }, // d_phi
|
||||
.k21_coeff = { 1.194019591863143e+02f, -7.294395555463191e+01f, -5.502401903255857e+00f, 1.056061060815681e+01f }, // theta
|
||||
.k22_coeff = { 1.953914704492318e+01f, -1.853477522511211e+01f, 5.522863854227361e+00f, 2.440732134290017e-01f }, // d_theta
|
||||
.k23_coeff = { -3.113451478996141e+01f, 3.917196236114254e+01f, -1.888229690044390e+01f, 4.105978636011114e+00f }, // x
|
||||
.k24_coeff = { -3.864995803122945e+01f, 4.873841855321316e+01f, -2.361615127826612e+01f, 5.240190452941591e+00f }, // d_x
|
||||
.k25_coeff = { 2.402815287983674e+02f, -2.353554477048087e+02f, 7.938206986720357e+01f, 7.179992245652441e-01f }, // phi
|
||||
.k26_coeff = { 5.110194134606057e+01f, -5.057473149871059e+01f, 1.733200146622070e+01f, -2.070892022734363e-01f }, // d_phi
|
||||
},
|
||||
.theta = 0.0f,
|
||||
.x = 0.0f,
|
||||
|
||||
@ -33,6 +33,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||
if (mode == g->mode)
|
||||
return GIMBAL_OK;
|
||||
|
||||
// 检查是否relax->absolute
|
||||
int relax_to_absolute = (g->mode == GIMBAL_MODE_RELAX && mode == GIMBAL_MODE_ABSOLUTE);
|
||||
|
||||
PID_Reset(&g->pid.yaw_angle);
|
||||
PID_Reset(&g->pid.yaw_omega);
|
||||
PID_Reset(&g->pid.pit_angle);
|
||||
@ -43,14 +46,13 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||||
MOTOR_DM_Enable(&(g->param->yaw_motor));
|
||||
|
||||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||||
// if (g->mode == GIMBAL_MODE_RELAX) {
|
||||
// if (mode == GIMBAL_MODE_ABSOLUTE) {
|
||||
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||
// } else if (mode == GIMBAL_MODE_RELATIVE) {
|
||||
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||
// }
|
||||
// }
|
||||
|
||||
if (relax_to_absolute) {
|
||||
// pitch回到限位中点
|
||||
g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
|
||||
} else {
|
||||
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
|
||||
}
|
||||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||||
|
||||
g->mode = mode;
|
||||
@ -253,7 +255,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||||
void Gimbal_Output(Gimbal_t *g){
|
||||
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
|
||||
MOTOR_MIT_Output_t output = {0};
|
||||
output.torque = g->out.yaw * 5.0f; // 乘以减速比
|
||||
output.torque = g->out.yaw * 5.0f;
|
||||
output.kd = 0.3f;
|
||||
MOTOR_RM_Ctrl(&g->param->pit_motor);
|
||||
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
|
||||
|
||||
@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
||||
|
||||
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
|
||||
memset(&s->output,0,sizeof(s->output));
|
||||
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */
|
||||
s->target_variable.target_rpm=4000.0f; /* 归一化目标转速 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -52,8 +52,8 @@ void Task_ctrl_gimbal(void *argument) {
|
||||
|
||||
Gimbal_UpdateFeedback(&gimbal);
|
||||
|
||||
// osMessageQueueReset(task_runtime.msgq.chassis_yaw);
|
||||
// osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
|
||||
osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
|
||||
osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
|
||||
|
||||
Gimbal_Control(&gimbal, &gimbal_cmd);
|
||||
|
||||
|
||||
@ -34,7 +34,7 @@ void Task_Init(void *argument) {
|
||||
/* 创建任务线程 */
|
||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||
task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
|
||||
// task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
|
||||
|
||||
@ -17,9 +17,9 @@ function K = get_k_length(leg_length)
|
||||
R1=0.068; %驱动轮半径
|
||||
L1=leg_length/2; %摆杆重心到驱动轮轴距离
|
||||
LM1=leg_length/2; %摆杆重心到其转轴距离
|
||||
l1=-0.03; %机体质心距离转轴距离
|
||||
l1=-0.01; %机体质心距离转轴距离
|
||||
mw1=0.60; %驱动轮质量
|
||||
mp1=1.8; %杆质量
|
||||
mp1=1.5; %杆质量
|
||||
M1=12.0; %机体质量
|
||||
Iw1=mw1*R1^2; %驱动轮转动惯量
|
||||
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
|
||||
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
|
||||
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
||||
B=double(B);
|
||||
|
||||
Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[250 0;0 50]; %T Tp
|
||||
Q=diag([500 1 600 200 8000 100]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[200 0;0 60]; %T Tp
|
||||
|
||||
K=lqr(A,B,Q,R);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user