修改平衡参数

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()
+{
+    
+}

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 // 速度测量噪声