换cpp架构前备份
This commit is contained in:
parent
f2e3b63eac
commit
eccdd5be2e
|
|
@ -0,0 +1,62 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/7.
|
||||
//
|
||||
|
||||
#include "estimator.h"
|
||||
static const float dt = 1e-3;
|
||||
//状态转移矩阵
|
||||
static float F[4] = {0,1,
|
||||
1,0};
|
||||
//控制矩阵
|
||||
static float B[2] = {dt,
|
||||
1};
|
||||
//观测矩阵
|
||||
static float H[4] = {1,0,
|
||||
0,1};
|
||||
//后验估计协方差初始值
|
||||
static float P[4] = {100, 0.1,
|
||||
0.1, 100};
|
||||
// P Q矩阵初始值（其实这里设置多少都无所谓）
|
||||
static float Q[4] = {0.01, 0,
|
||||
0, 0.01};
|
||||
static float R[4] = {100000, 0,
|
||||
0, 100000,};
|
||||
estimator::estimator(float process_noise, float measure_noise) {
|
||||
Kalman_Filter_Init(&EstimateKF_,2,1,2);
|
||||
EstimateKF_.UseAutoAdjustment = 1;
|
||||
|
||||
for (uint8_t i = 0; i < 4; i += 3)
|
||||
{
|
||||
// 初始化过程噪声与量测噪声
|
||||
Q[i] = process_noise;
|
||||
R[i] = measure_noise;
|
||||
}
|
||||
memcpy(EstimateKF_.F_data,F,sizeof(F));
|
||||
memcpy(EstimateKF_.B_data,B,sizeof(B));
|
||||
memcpy(EstimateKF_.H_data,H,sizeof(H));
|
||||
memcpy(EstimateKF_.Q_data,Q,sizeof(Q));
|
||||
memcpy(EstimateKF_.R_data,R,sizeof(R));
|
||||
|
||||
DWT_GetDeltaT(&DWT_CNT_);
|
||||
}
|
||||
|
||||
void estimator::update(float x,float x_dot,float ax) {
|
||||
EstimateKF_.MeasuredVector[0] = x;
|
||||
EstimateKF_.MeasuredVector[1] = x_dot;
|
||||
|
||||
EstimateKF_.ControlVector[0] = ax;
|
||||
|
||||
Kalman_Filter_Update(&EstimateKF_);
|
||||
|
||||
// 提取估计值
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
Estimate_X_[i] = EstimateKF_.FilteredValue[i];
|
||||
}
|
||||
}
|
||||
|
||||
void estimator::get_result(float state[2])
|
||||
{
|
||||
state[0] = Estimate_X_[0];
|
||||
state[1] = Estimate_X_[1];
|
||||
}
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/7.
|
||||
//
|
||||
#include "kalman_filter.h"
|
||||
#include "bsp_dwt.h"
|
||||
#ifndef WHEEL_LEGGED_ESTIMATOR_H
|
||||
#define WHEEL_LEGGED_ESTIMATOR_H
|
||||
|
||||
|
||||
class estimator {
|
||||
public:
|
||||
estimator(float process_noise, float measure_noise);
|
||||
void get_result(float state[2]);
|
||||
void update(float x,float x_dot,float ax);
|
||||
|
||||
private:
|
||||
KalmanFilter_t EstimateKF_; //使用KF作为状态观测器
|
||||
float Estimate_X_[2]; //观测器估计的状态量
|
||||
uint32_t DWT_CNT_; //计时用
|
||||
float dt_;
|
||||
};
|
||||
|
||||
|
||||
#endif //WHEEL_LEGGED_ESTIMATOR_H
|
||||
|
|
@ -147,7 +147,6 @@ float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {
|
|||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -113,30 +113,30 @@ void mylqr_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) {
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@ INS_t *Chassis_IMU_data;
|
|||
#ifdef ONE_BOARD
|
||||
static Publisher_t *chassis_pub; // 用于发布底盘的数据
|
||||
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
|
||||
INS_t *Chassis_IMU_data;
|
||||
#endif // !ONE_BOARD
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
|
||||
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
|
||||
|
|
@ -120,12 +121,13 @@ void ChassisInit()
|
|||
//@todo:瓴控驱动代码有些问题 1号电机必须先初始化
|
||||
|
||||
wheel_motor_config.can_init_config.tx_id = 1;
|
||||
wheel_motor_l = LKMotorInit(&wheel_motor_config);
|
||||
|
||||
wheel_motor_config.can_init_config.tx_id = 2;
|
||||
wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
wheel_motor_r = LKMotorInit(&wheel_motor_config);
|
||||
|
||||
wheel_motor_config.can_init_config.tx_id = 2;
|
||||
wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
wheel_motor_l = LKMotorInit(&wheel_motor_config);
|
||||
|
||||
|
||||
|
||||
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
|
|
@ -191,7 +193,8 @@ void ChassisInit()
|
|||
.Ki = 0,
|
||||
.Kd = 0.0f,
|
||||
.MaxOut = 2,
|
||||
.Improve = PID_Derivative_On_Measurement,
|
||||
.Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 0.001f
|
||||
};
|
||||
PIDInit(&Turn_Loop_PID,&turn_pid_cfg);
|
||||
|
||||
|
|
@ -404,7 +407,7 @@ void ChassisTask()
|
|||
|
||||
|
||||
|
||||
target_yaw = chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z]
|
||||
target_yaw = 0.0f;//chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z]
|
||||
|
||||
//float turn_T = -PIDCalculate(&Turn_Loop_PID,chassis_cmd_recv.offset_angle,0);
|
||||
//float turn_T = -1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
|
|
@ -438,10 +441,17 @@ void ChassisTask()
|
|||
|
||||
HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
|
||||
HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
|
||||
// HTMotorSetRef(motor_rf,0);
|
||||
// HTMotorSetRef(motor_rb,0);
|
||||
//
|
||||
// HTMotorSetRef(motor_lf,0);
|
||||
// HTMotorSetRef(motor_lb,0);
|
||||
}
|
||||
|
||||
LKMotorSetRef(wheel_motor_r,wheel_current_r);
|
||||
LKMotorSetRef(wheel_motor_l,wheel_current_l);
|
||||
// LKMotorSetRef(wheel_motor_r,100);
|
||||
// LKMotorSetRef(wheel_motor_l,100);
|
||||
|
||||
Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0);
|
||||
Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current/HT_CMD_COEF,motor_rb->measure.real_current/HT_CMD_COEF);
|
||||
|
|
|
|||
|
|
@ -17,8 +17,8 @@
|
|||
#include "stdint.h"
|
||||
|
||||
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
|
||||
//#define ONE_BOARD // 单板控制整车
|
||||
#define CHASSIS_BOARD //底盘板
|
||||
#define ONE_BOARD // 单板控制整车
|
||||
//#define CHASSIS_BOARD //底盘板
|
||||
// #define GIMBAL_BOARD //云台板
|
||||
|
||||
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
|
||||
|
|
|
|||
Loading…
Reference in New Issue