From 3e54d14737987eab7b5e1877425bc585f29fe315 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Sat, 9 Mar 2024 22:32:37 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=8D=A2cpp=E6=9E=B6=E6=9E=84=20?= =?UTF-8?q?=E6=9C=89=E6=A0=87=E5=87=86=E5=86=B2=E7=AA=81=E5=BE=85=E8=A7=A3?= =?UTF-8?q?=E5=86=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Src/{main.c => main.cpp} | 0 application/balance/balance.cpp | 32 +++++++++---------- .../chassis/{chassis.c => chassis.cpp} | 14 +++++--- application/chassis/chassis.h | 2 ++ application/{robot.c => robot.cpp} | 0 5 files changed, 27 insertions(+), 21 deletions(-) rename Src/{main.c => main.cpp} (100%) rename application/chassis/{chassis.c => chassis.cpp} (98%) rename application/{robot.c => robot.cpp} (100%) diff --git a/Src/main.c b/Src/main.cpp similarity index 100% rename from Src/main.c rename to Src/main.cpp diff --git a/application/balance/balance.cpp b/application/balance/balance.cpp index be07ae2..547b69f 100644 --- a/application/balance/balance.cpp +++ b/application/balance/balance.cpp @@ -71,30 +71,30 @@ void get_lqr_k(float L0, float K[12]) */ t2 = L0 * L0; t3 = t2 * L0; - /* 24-Jan-2024 14:05:09 */ - K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F; + /* 06-Mar-2024 21:40:29 */ + K_temp[0] = + ((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F; K_temp[1] = - ((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F; + ((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F; K_temp[2] = - ((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F; + ((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F; K_temp[3] = - ((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F; - K_temp[4] = - ((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F; + ((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F; + K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F; K_temp[5] = - ((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F; - K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) - - 10.4576302F; + ((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F; + K_temp[6] = + ((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F; K_temp[7] = - ((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F; + ((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F; K_temp[8] = - ((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F; + ((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F; K_temp[9] = - ((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F; - K_temp[10] = - ((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F; + ((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F; + K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) + + 5.52106953F; K_temp[11] = - ((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F; + ((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F; //matlab赋值顺序不同 for (int i = 0; i < 6; ++i) { diff --git a/application/chassis/chassis.c b/application/chassis/chassis.cpp similarity index 98% rename from application/chassis/chassis.c rename to application/chassis/chassis.cpp index 5cddf98..1ee3151 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.cpp @@ -22,7 +22,7 @@ #include "power_meter.h" #include "ins_task.h" -#include "balance_old.h" +#include "balance.h" #include "observer.h" #include "general_def.h" @@ -31,12 +31,14 @@ #include "arm_math.h" #include "vofa.h" #include "user_lib.h" +#include "balance.h" /* 根据robot_def.h中的macro自动计算的参数 */ #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长 /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */ + #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度 #include "can_comm.h" #include "ins_task.h" @@ -65,9 +67,10 @@ static LKMotorInstance *wheel_motor_r,*wheel_motor_l; static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 -static LegInstance legInstance; -Balance_Control_t BalanceControl; + + +balance balance_chassis(); BalanceObserver Observer; @@ -75,6 +78,7 @@ PIDInstance Turn_Loop_PID , Roll_Loop_PID; Chassis_state_e ChassisState; + void ChassisInit() { // 四个关节电机的参数,改tx_id和反转标志位即可 @@ -176,7 +180,7 @@ void ChassisInit() .Ki = 0, .Kd = 3, .MaxOut = 3, - .Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter, + .Improve = static_cast(PID_Derivative_On_Measurement | PID_DerivativeFilter), .Derivative_LPF_RC = 0.01f, }, .LQR_state_num = 6, @@ -185,7 +189,7 @@ void ChassisInit() }; //Leg_Init(&legInstance,&leg_r_init_conf); - Balance_Control_Init(&BalanceControl,balanceInitConfig); + Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 //转向环 PID_Init_Config_s turn_pid_cfg = { diff --git a/application/chassis/chassis.h b/application/chassis/chassis.h index b8b95bd..4ca0cdf 100644 --- a/application/chassis/chassis.h +++ b/application/chassis/chassis.h @@ -13,6 +13,8 @@ void ChassisInit(); */ void ChassisTask(); + + typedef enum { FAIL = 0, diff --git a/application/robot.c b/application/robot.cpp similarity index 100% rename from application/robot.c rename to application/robot.cpp