From be23c6c88cb7881eeab07ddc330bfd40958cdb56 Mon Sep 17 00:00:00 2001 From: kai <1797003616@qq.com> Date: Sat, 23 Mar 2024 16:10:20 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=B9=B3=E8=A1=A1=E5=8F=82?= =?UTF-8?q?=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 106 ++++++++++++++++++++++++++++++++++ application/chassis/balance.h | 22 ++++--- modules/imu/ins_task.c | 6 +- modules/imu/ins_task.h | 2 +- 4 files changed, 120 insertions(+), 16 deletions(-) create mode 100644 application/chassis/balance.c diff --git a/application/chassis/balance.c b/application/chassis/balance.c new file mode 100644 index 0000000..9b3466a --- /dev/null +++ b/application/chassis/balance.c @@ -0,0 +1,106 @@ +// app +#include "balance.h" +#include "linkNleg.h" +#include "robot_def.h" +#include "general_def.h" +#include "ins_task.h" +#include "HT04.h" +#include "LK9025.h" +#include "controller.h" +#include "can_comm.h" +#include "super_cap.h" +#include "user_lib.h" +#include "remote_control.h" +#include "referee_task.h" +#include "stdint.h" +#include "arm_math.h" // 需要用到较多三角函数 +#include "bsp_dwt.h" +#include "bsp_log.h" + +// 计时变量 +static uint32_t balance_dwt_cnt; +static float del_t; + +// 底盘拥有的实例模块 +static INS_t *Chassis_IMU_data; +static RC_ctrl_t *rc_data; // 底盘单独调试用 +static Chassis_Ctrl_Cmd_s chassis_cmd_recv; + +// 四个关节电机和两个驱动轮电机 +static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 +static LKMotorInstance *l_driven, *r_driven, *driven[2]; + +// 两个腿的参数,0为左腿,1为右腿 +static LinkNPodParam l_side, r_side; +static ChassisParam chassis; + +// 底盘状态 +static Robot_Status_e chassis_status; + + +void BalanceInit() +{ + rc_data = RemoteControlInit(&huart3); + Chassis_IMU_data = INS_Init(); + + // 关节电机 + Motor_Init_Config_s joint_conf = { + // 写一个,剩下的修改方向和id即可 + .can_init_config = { + .can_handle = &hcan1}, + .controller_setting_init_config = { + .close_loop_type = OPEN_LOOP, + .outer_loop_type = OPEN_LOOP, + .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL, + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + }, + .motor_type = HT04}; + joint_conf.can_init_config.tx_id = 1; + joint_conf.can_init_config.rx_id = 11; + joint[LF] = lf = HTMotorInit(&joint_conf); + joint_conf.can_init_config.tx_id = 2; + joint_conf.can_init_config.rx_id = 12; + joint[LB] = lb = HTMotorInit(&joint_conf); + joint_conf.can_init_config.tx_id = 3; + joint_conf.can_init_config.rx_id = 13; + joint[RF] = rf = HTMotorInit(&joint_conf); + joint_conf.can_init_config.tx_id = 4; + joint_conf.can_init_config.rx_id = 14; + joint[RB] = rb = HTMotorInit(&joint_conf); + + // 驱动轮电机 + Motor_Init_Config_s driven_conf = { + // 写一个,剩下的修改方向和id即可 + .can_init_config.can_handle = &hcan2, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = OPEN_LOOP, + .close_loop_type = OPEN_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = LK9025, + }; + driven_conf.can_init_config.tx_id = 2; + driven[LD] = l_driven = LKMotorInit(&driven_conf); + driven_conf.can_init_config.tx_id = 1; + driven[RD] = r_driven = LKMotorInit(&driven_conf); + + // 状态初始化 + chassis_status = ROBOT_READY; + DWT_GetDeltaT(&balance_dwt_cnt); +} + +static void EnableAllMotor() /* 打开所有电机 */ +{ + for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机 + HTMotorEnable(joint[i]); + for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机 + LKMotorEnable(driven[i]); +} + +void BalanceTask() +{ + +} \ No newline at end of file diff --git a/application/chassis/balance.h b/application/chassis/balance.h index 5703405..5af6f83 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -1,23 +1,21 @@ #pragma once // 底盘参数 -#define CALF_LEN 0.245f // 小腿 -#define THIGH_LEN 0.14f // 大腿 -#define JOINT_DISTANCE 0.108f // 关节间距 -#define WHEEL_RADIUS 0.078f // 轮子半径 -#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble -#define WHEEL_DISTANCE 0.48f // 轮子间距 +#define CALF_LEN 0.24f // 小腿 +#define THIGH_LEN 0.14f // 大腿 +#define JOINT_DISTANCE 0.11f // 关节间距 +#define WHEEL_RADIUS 0.06925f // 轮子半径 +#define LIMIT_LINK_RAD 0.205467224 // 初始限位角度,见ParamAssemble #define BALANCE_GRAVITY_BIAS 0 -#define ROLL_GRAVITY_BIAS 0.03f +#define ROLL_GRAVITY_BIAS 0 #define MAX_ACC_REF 0.7f #define MAX_DIST_TRACK 0.1f #define MAX_VEL_TRACK 0.5f -#define CENTER_IMU_R 0.13f // IMU距离中心的距离 -#define CENTER_IMU_W 0.11f -#define CENTER_IMU_L 0.074f -#define CENTER_IMU_H 0.060f -#define CENTER_IMU_THETA 0.9768f +#define CENTER_IMU_R 0.09435f // IMU距离中心的距离 +#define CENTER_IMU_W 0 +#define CENTER_IMU_L 0.09435f +#define CENTER_IMU_H 0 #define VEL_PROCESS_NOISE 25 // 速度过程噪声 #define VEL_MEASURE_NOISE 800 // 速度测量噪声 diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 6e5ce71..029b0ab 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -77,12 +77,12 @@ static void InitQuaternion(float *init_q4) init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量) } -attitude_t *INS_Init(void) +INS_t *INS_Init(void) { if (!INS.init) INS.init = 1; else - return (attitude_t *)&INS.Gyro; + return &INS; HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1); @@ -113,7 +113,7 @@ attitude_t *INS_Init(void) INS.AccelLPF = 0.0085; INS.DGyroLPF = 0.009; DWT_GetDeltaT(&INS_DWT_Count); - return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复. + return &INS; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复. } /* 注意以1kHz的频率运行此任务 */ diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index 46772a2..5c62852 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -84,7 +84,7 @@ typedef struct * @brief 初始化惯导解算系统 * */ -attitude_t *INS_Init(void); +INS_t *INS_Init(void); /** * @brief 此函数放入实时系统中,以1kHz频率运行