robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: robofish:1712a3da8c4074fedefc9dfa8e53aeda5597be74
Branches Tags
robofish:main
robofish:MC_02
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
..
compare: robofish:39adf0a2b68ff0c2f9bcf3fda2d85564e10f1c6e
Branches Tags
robofish:MC_02
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
robofish:main

No commits in common. "1712a3da8c4074fedefc9dfa8e53aeda5597be74" and "39adf0a2b68ff0c2f9bcf3fda2d85564e10f1c6e" have entirely different histories.
1712a3da8c ... 39adf0a2b6

13 changed files with 8634 additions and 9563 deletions
Show Stats Expand all files Collapse all files

62
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -135,7 +135,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S0 -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 -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>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -157,22 +157,6 @@
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>255</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134218668</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename>startup_stm32h723xx.s</Filename>
<ExecCommand></ExecCommand>
<Expression>\\CtrBoard_H7_ALL\startup_stm32h723xx.s\255</Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>0</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>128</Address>
@ -187,7 +171,7 @@
<Expression>0x00000080</Expression>
</Bp>
<Bp>
<Number>2</Number>
<Number>1</Number>
<Type>0</Type>
<LineNumber>0</LineNumber>
<EnabledFlag>1</EnabledFlag>
@ -224,6 +208,46 @@
<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>
@ -272,7 +296,7 @@
<EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog>
<Protocol>2</Protocol>
<DbgClock>1000000</DbgClock>
<DbgClock>5000000</DbgClock>
</DebugDescription>
</TargetOption>
</Target>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

16890
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

152
User/device/ai.c
View File

@ -1,152 +0,0 @@
/*
AI
*/
/* Includes ----------------------------------------------------------------- */
#include "ai.h"
#include <string.h>
#include "bsp\delay.h"
#include "bsp\uart.h"
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
#include "component\filter.h"
/* Private define ----------------------------------------------------------- */
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[AI_LEN_RX_BUFF];
static bool inited = false;
static osThreadId_t thread_alert;
/* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
static void Ai_IdleLineCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) {
UNUSED(ai);
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED;
VERIFY((thread_alert = osThreadGetId()) != NULL);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
Ai_IdleLineCallback);
inited = true;
return 0;
}
int8_t AI_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
return DEVICE_OK;
}
int8_t AI_StartReceiving(AI_t *ai) {
UNUSED(ai);
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
AI_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool AI_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
SIGNAL_AI_RAW_REDY);
}
int8_t AI_ParseHost(AI_t *ai) {
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
goto error;
ai->ai_online = true;
memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
memset(rxbuf, 0, AI_LEN_RX_BUFF);
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
}
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (ai == NULL) return DEVICE_ERR_NULL;
if (cmd_host == NULL) return DEVICE_ERR_NULL;
ai->ai_online = false;
memset(&(ai->from_host), 0, sizeof(ai->from_host));
memset(cmd_host, 0, sizeof(*cmd_host));
return 0;
}
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
ai->to_host.mcu.id = AI_ID_MCU;
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
sizeof(*quat));
ai->to_host.mcu.package.data.notice = 0;
if (ai->status == AI_STATUS_AUTOAIM)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
else if (ai->status == AI_STATUS_HITSWITCH)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
else if (ai->status == AI_STATUS_AUTOMATIC)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
ai->to_host.mcu.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.mcu.package),
sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
(void)ref;
ai->to_host.ref.id = AI_ID_REF;
ai->to_host.ref.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.ref.package),
sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai, bool ref_update) {
if (ref_update) {
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
(uint8_t *)&(ai->to_host.mcu),
sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

69
User/device/ai.h
View File

@ -1,69 +0,0 @@
/*
AI
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#include "component\ahrs.h"
#include "component\cmd.h"
#include "component\user_math.h"
#include "component\filter.h"
#include "device\device.h"
#include "device\referee.h"
#include "protocol.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t id;
Protocol_UpPackageReferee_t package;
} AI_UpPackageReferee_t;
typedef struct __packed {
uint8_t id;
Protocol_UpPackageMCU_t package;
} AI_UpPackageMCU_t;
typedef struct __packed {
osThreadId_t thread_alert;
Protocol_DownPackage_t from_host;
struct {
AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu;
} to_host;
CMD_AI_Status_t status;
bool ai_online;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AI_Init(AI_t *ai);
int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai);
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
int8_t AI_StartSend(AI_t *ai, bool option);
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
#ifdef __cplusplus
}
#endif

917
User/module/balance_chassis.c
View File

File diff suppressed because it is too large Load Diff

27
User/module/balance_chassis.h
View File

@ -38,10 +38,19 @@ extern "C" {
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
} 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; /* 底盘运动向量 */
@ -117,6 +126,13 @@ 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位置 */
@ -135,6 +151,8 @@ typedef struct {
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;
@ -151,9 +169,8 @@ typedef struct {
/* 滤波器 */
struct {
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
LowPassFilter2p_t velocity_est; /* 速度估计滤波器 */
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
} filter;
} Chassis_t;

46
User/module/config.c
View File

@ -24,7 +24,7 @@ Config_RobotParam_t robot_config = {
.gimbal_param = {
.pid = {
.yaw_omega = {
.k = 2.0f,
.k = 1.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 = 7.0f,
.p = 3.5f,
.k = 8.0f,
.p = 3.0f,
.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 = 2.5f,
.k = 5.0f,
.p = 5.0f,
.i = 0.2f,
.d = 0.01f,
.i = 2.5f,
.d = 0.0f,
.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.2f,
.pit = 2.12f,
},
.travel = {
.yaw = -1.0f,
.pit = 0.85f,
.pit = 0.9f,
},
.low_pass_cutoff_freq = {
.out = -1.0f,
@ -180,7 +180,7 @@ Config_RobotParam_t robot_config = {
.chassis_param = {
.yaw={
.k=1.0f,
.k=0.0f,
.p=1.0f,
.i=0.0f,
.d=0.3f,
@ -189,7 +189,7 @@ Config_RobotParam_t robot_config = {
.d_cutoff_freq=30.0f,
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
},
.roll={
.k=0.2f,
.p=0.5f, /* 横滚角比例系数 */
@ -297,7 +297,7 @@ Config_RobotParam_t robot_config = {
},
},
.mech_zero_yaw = 1.78040409f, /* 机械零点 */
.mech_zero_yaw = 4.34085676f, /* 250.5度，机械零点 */
.vmc_param = {
{ // 左腿
@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
},
.lqr_gains = {
.k11_coeff = { -2.120324305163214e+02f, 2.384281822810979e+02f, -1.162210131498081e+02f, -2.405740963481636e+00f }, // theta
.k12_coeff = { 4.320836680770054e-01f, 1.360781729068518e-01f, -8.343612068216379e+00f, -8.600090910123033e-02f }, // d_theta
.k13_coeff = { -4.493046202256553e+01f, 4.577344792125822e+01f, -1.629835599958664e+01f, -1.007247166173255e+00f }, // x
.k14_coeff = { -4.823335755850488e+01f, 4.987409533322209e+01f, -1.917162943665977e+01f, -1.473642463713354e+00f }, // d_x
.k15_coeff = { -4.920796990864941e+01f, 6.450325939254844e+01f, -3.268284723443183e+01f, 7.841340265493823e+00f }, // phi
.k16_coeff = { -1.264530042590822e+01f, 1.663296191858249e+01f, -8.467446023207946e+00f, 2.096442008644146e+00f }, // d_phi
.k21_coeff = { 1.636475235954215e+02f, -1.128937920212271e+02f, 4.887401528348596e+00f, 1.459120525493287e+01f }, // theta
.k22_coeff = { 9.939826270288583e+00f, -8.353709902293666e+00f, 1.640639416293288e+00f, 1.492185865897709e+00f }, // d_theta
.k23_coeff = { -4.968599085108445e+01f, 6.646556717472484e+01f, -3.397333983104631e+01f, 7.847958130301292e+00f }, // x
.k24_coeff = { -7.188031995054928e+01f, 9.082377283123918e+01f, -4.412648091833633e+01f, 9.959213854333724e+00f }, // d_x
.k25_coeff = { 2.360507220981238e+02f, -2.398095392324340e+02f, 8.536061841837561e+01f, 2.469595316477948e+00f }, // phi
.k26_coeff = { 6.296425580760564e+01f, -6.412220242164098e+01f, 2.293354052056732e+01f, 4.870876539985159e-01f }, // d_phi
.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
},
.theta = 0.0f,
.x = 0.0f,

22
User/module/gimbal.c
View File

@ -33,9 +33,6 @@ 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);
@ -44,15 +41,16 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
MOTOR_DM_Enable(&(g->param->yaw_motor));
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
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;
}
// 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;
// }
// }
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->mode = mode;
@ -255,7 +253,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);

2
User/module/shoot.c
View File

@ -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=4000.0f; /* 归一化目标转速 */
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */
return 0;
}

4
User/task/ctrl_gimbal.c
View File

@ -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);

2
User/task/init.c
View File

@ -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);

4
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -17,7 +17,7 @@ function K = get_k_length(leg_length)
R1=0.068; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.01; %机体质心距离转轴距离
l1=-0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量
M1=12.0; %机体质量
@ -48,7 +48,7 @@ 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 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
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
K=lqr(A,B,Q,R);