Hshine
/
wheel_legged
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

merge into: Hshine:main
Branches Tags
Hshine:main
Hshine:cpp
Hshine:master
...
pull from: Hshine:cpp
Branches Tags
Hshine:main
Hshine:cpp
Hshine:master

2 Commits
main ... cpp

Author SHA1 Message Date
宋家齐 58c1d73ca6 更换cpp架构 有标准冲突待解决 2024-03-09 22:56:07 +08:00
宋家齐 3e54d14737 更换cpp架构 有标准冲突待解决 2024-03-09 22:32:37 +08:00
8 changed files with 71 additions and 69 deletions
Show Stats Expand all files Collapse all files

0
Src/main.c → Src/main.cpp
View File

41
application/balance/balance.cpp
View File

@ -61,6 +61,15 @@ Matrixf<2,1> balance::get_control() {
return balance_LQR_.get_control();
}
void balance::L_target_set(float L) {
target_length_ = L;
}
void balance::Phi_target_set(float Phi) {
}
void get_lqr_k(float L0, float K[12])
{
float t2;
@ -71,30 +80,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) {

6
application/balance/balance.h
View File

@ -16,10 +16,14 @@ typedef struct
class balance {
public:
balance(Balance_Init_config_s* InitConfig);
balance()= default;
explicit balance(Balance_Init_config_s* InitConfig);
void feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
void control_loop();
void target_set(float x);
void L_target_set(float L);
void Phi_target_set(float Phi);
Matrixf<2,1> get_control();
private:

1
application/balance/leg.cpp
View File

@ -153,3 +153,4 @@ float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {

1
application/balance/leg.h
View File

@ -40,6 +40,7 @@ class leg
{
public:
leg(Leg_init_config_s* legInitConfig);
leg() {};
void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
void input_update(float F,float Tp);
void feedback_update(float T1_fb,float T2_fb);

75
application/chassis/chassis.c → application/chassis/chassis.cpp
View File

@ -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,7 @@ 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,29 +75,30 @@ PIDInstance Turn_Loop_PID , Roll_Loop_PID;
Chassis_state_e ChassisState;
void ChassisInit()
{
// 四个关节电机的参数,改tx_id和反转标志位即可
Motor_Init_Config_s chassis_motor_config = {
.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,//| CURRENT_LOOP,
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
},
.motor_type = HT04,
};
.can_init_config = {.can_handle = &hcan2},
};
Motor_Init_Config_s wheel_motor_config = {
.can_init_config.can_handle = &hcan1,
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
},
.motor_type = LK9025,
.can_init_config = {.can_handle = &hcan1},
};
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
chassis_motor_config.can_init_config.tx_id = 1;
@ -155,18 +156,22 @@ void ChassisInit()
.Ki = 100,//5,
.Kd = 20,//10,//6,//6,
.MaxOut = 100,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
.Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
PID_Derivative_On_Measurement |
PID_DerivativeFilter),
.IntegralLimit = 100,
.Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
},
.phi0_PID_conf = {
.Kp = 80,
.Ki = 0,
.Kd = 5,
.MaxOut = 5,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement| PID_DerivativeFilter,
.Derivative_LPF_RC = 1/(2*PI*10),
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
PID_Derivative_On_Measurement |
PID_DerivativeFilter),
.IntegralLimit = 100,
.Derivative_LPF_RC = 1/(2*PI*10),
},
.init_target_L0 = 0.15f,
.F_feedforward = 0,
@ -176,16 +181,13 @@ void ChassisInit()
.Ki = 0,
.Kd = 3,
.MaxOut = 3,
.Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter,
.Improve = static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
.Derivative_LPF_RC = 0.01f,
},
.LQR_state_num = 6,
.LQR_control_num = 2,
};
//Leg_Init(&legInstance,&leg_r_init_conf);
Balance_Control_Init(&BalanceControl,balanceInitConfig);
balance_chassis = balance(&balanceInitConfig);
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
//转向环
PID_Init_Config_s turn_pid_cfg = {
@ -193,7 +195,7 @@ void ChassisInit()
.Ki = 0,
.Kd = 0.0f,
.MaxOut = 2,
.Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter,
.Improve = static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
.Derivative_LPF_RC = 0.001f
};
PIDInit(&Turn_Loop_PID,&turn_pid_cfg);
@ -285,10 +287,7 @@ static void Chassis_State_update()
float x_dot = speed * 0.135f/2;
Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current / HT_CMD_COEF,motor_rb->measure.real_current / HT_CMD_COEF);
Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current / HT_CMD_COEF,motor_lb->measure.real_current / HT_CMD_COEF);
balance_chassis.feedback_update(leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
if( ChassisState == FAIL )
{
@ -369,27 +368,13 @@ void ChassisTask()
float vofa_send_data[8];
BalanceControl.leg_right.target_L0 += chassis_cmd_recv.vy/5280 * 0.0003f;
BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f);
BalanceControl.leg_right.target_phi0 = PI/2;
static float L_target = 0;
L_target += chassis_cmd_recv.vy/5280 * 0.0003f;
L_target = float_constrain(L_target,0.15f,0.30f);
BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0;
BalanceControl.leg_left.target_L0 = float_constrain(BalanceControl.leg_left.target_L0,0.15f,0.30f);
BalanceControl.leg_left.target_phi0 = PI/2;
balance_chassis.L_target_set(L_target);
vofa_send_data[0] = BalanceControl.leg_right.legState.L0;
vofa_send_data[1] = BalanceControl.leg_left.legState.L0;
vofa_send_data[2] = BalanceControl.leg_length_PID.Ref;
vofa_send_data[3] = BalanceControl.leg_length_PID.Measure;
vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
vofa_send_data[5] = BalanceControl.leg_left.legInput.F;
vofa_send_data[6] = Observer.Estimate_X[3];
vofa_send_data[7] = BalanceControl.state[3];
vofa_justfloat_output(vofa_send_data,32,&huart1);
//vofa_justfloat_output(vofa_send_data,32,&huart1);
static float target_x = 0;
static float target_yaw = 0;

2
application/chassis/chassis.h
View File

@ -13,6 +13,8 @@ void ChassisInit();
*/
void ChassisTask();
typedef enum
{
FAIL = 0,

0
application/robot.c → application/robot.cpp
View File