diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 3f2933e..d58294c 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -28,10 +28,10 @@ #define STOOL_YAW_RATE_REF 3.5f #define STOOL_ACC_REF 4.0f #define STOOL_BRAKE_ACC_REF 8.0f -#define STOOL_PITCH_K 36.0f -#define STOOL_PITCH_W_K 10.0f -#define STOOL_VEL_K 6.0f -#define STOOL_STOP_DIST_K 3.0f +#define STOOL_PITCH_K 30.0f +#define STOOL_PITCH_W_K 8.5f +#define STOOL_VEL_K 4.5f +#define STOOL_STOP_DIST_K 2.0f #define STOOL_VEL_LPF_RC 0.03f #define STOOL_WHEEL_TORQUE_LIMIT 25.0f #define STOOL_RC_DEADBAND 40 @@ -236,9 +236,9 @@ void BalanceInit() }; driven_conf.can_init_config.tx_id = 1; - driven[RD] = r_driven = LKMotorInit(&driven_conf); - driven_conf.can_init_config.tx_id = 2; driven[LD] = l_driven = LKMotorInit(&driven_conf); + driven_conf.can_init_config.tx_id = 2; + driven[RD] = r_driven = LKMotorInit(&driven_conf); // 腿长控制 PID_Init_Config_s leg_length_pid_conf = { diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 7fd7d3b..54f593e 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/modules/algorithm/QuaternionEKF.c @@ -199,7 +199,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, // 利用四元数反解欧拉角 QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f; - QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; + QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)