底盘状态机完善
This commit is contained in:
parent
eccdd5be2e
commit
23feabe1fc
|
|
@ -35,6 +35,7 @@ add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-le
|
|||
# Enable assembler files preprocessing
|
||||
add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>)
|
||||
|
||||
|
||||
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
||||
message(STATUS "Maximum optimization for speed")
|
||||
add_compile_options(-Ofast)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,56 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/7.
|
||||
//
|
||||
|
||||
#include "estimator.h"
|
||||
static const float dt = 1e-3f;
|
||||
//状态转移矩阵
|
||||
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.1f,
|
||||
0.1f, 100};
|
||||
// R Q矩阵初始值（其实这里设置多少都无所谓）
|
||||
static float Q[4] = {0.01f, 0,
|
||||
0, 0.01f};
|
||||
static float R[4] = {100000, 0,
|
||||
0, 100000,};
|
||||
void estimator_init(estimator_t *est ,float process_noise, float measure_noise) {
|
||||
Kalman_Filter_Init(&est->EstimateKF_,2,1,2);
|
||||
est->EstimateKF_.UseAutoAdjustment = 1;
|
||||
|
||||
for (uint8_t i = 0; i < 4; i += 3)
|
||||
{
|
||||
// 初始化过程噪声与量测噪声
|
||||
Q[i] = process_noise;
|
||||
R[i] = measure_noise;
|
||||
}
|
||||
memcpy(est->EstimateKF_.F_data,F,sizeof(F));
|
||||
memcpy(est->EstimateKF_.B_data,B,sizeof(B));
|
||||
memcpy(est->EstimateKF_.H_data,H,sizeof(H));
|
||||
memcpy(est->EstimateKF_.Q_data,Q,sizeof(Q));
|
||||
memcpy(est->EstimateKF_.R_data,R,sizeof(R));
|
||||
|
||||
DWT_GetDeltaT(&est->DWT_CNT_);
|
||||
}
|
||||
|
||||
void estimator_update(estimator_t *est ,float x,float x_dot,float ax) {
|
||||
est->EstimateKF_.MeasuredVector[0] = x;
|
||||
est->EstimateKF_.MeasuredVector[1] = x_dot;
|
||||
|
||||
est->EstimateKF_.ControlVector[0] = ax;
|
||||
|
||||
Kalman_Filter_Update(&est->EstimateKF_);
|
||||
|
||||
// 提取估计值
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
|
||||
}
|
||||
}
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
//
|
||||
// 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];
|
||||
}
|
||||
|
|
@ -6,19 +6,15 @@
|
|||
#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:
|
||||
typedef struct
|
||||
{
|
||||
KalmanFilter_t EstimateKF_; //使用KF作为状态观测器
|
||||
float Estimate_X_[2]; //观测器估计的状态量
|
||||
uint32_t DWT_CNT_; //计时用
|
||||
float dt_;
|
||||
};
|
||||
}estimator_t;
|
||||
|
||||
void estimator_init(estimator_t *est ,float process_noise, float measure_noise);
|
||||
void estimator_update(estimator_t *est ,float x,float x_dot,float ax);
|
||||
|
||||
#endif //WHEEL_LEGGED_ESTIMATOR_H
|
||||
|
|
|
|||
|
|
@ -4,10 +4,12 @@
|
|||
|
||||
#include "balance_old.h"
|
||||
|
||||
static void mylqr_k(float L0, float K[12]);
|
||||
static void mylqr_k(float L0, float K_mat[12]);
|
||||
|
||||
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
|
||||
{
|
||||
|
||||
balanceControl->target_L0 = 0.15f;
|
||||
Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
|
||||
Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
|
||||
|
||||
|
|
@ -15,6 +17,9 @@ void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_
|
|||
PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config);
|
||||
|
||||
PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf);
|
||||
|
||||
estimator_init(&balanceControl->v_estimator,100,1);
|
||||
Observer_Init(&balanceControl->state_observer);
|
||||
memset(balanceControl->state,0,6);
|
||||
memset(balanceControl->target_state,0,6);
|
||||
}
|
||||
|
|
@ -23,28 +28,47 @@ void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_
|
|||
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb);
|
||||
|
||||
// leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l}
|
||||
void Balance_feedback_update(Balance_Control_t* balanceControl,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 Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,const float MotionAccel[3])
|
||||
{
|
||||
Leg_State_update(&balanceControl->leg_right,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
||||
Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
||||
LegInstance *leg_r = &balanceControl->leg_right;
|
||||
LegInstance *leg_l = &balanceControl->leg_left;
|
||||
|
||||
Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
||||
Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
||||
|
||||
float K_matrix[12]={0};
|
||||
float L_average = (balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
|
||||
//(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
|
||||
float alpha = PI/2 - balanceControl->leg_right.legState.phi0;
|
||||
|
||||
float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
|
||||
//(leg_r->legState.L0 + leg_l->legState.L0)/2;
|
||||
float alpha = PI/2 - leg_r->legState.phi0;
|
||||
float theta = alpha - body_phi;
|
||||
|
||||
// static uint32_t dot_cnt = 0;
|
||||
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
// float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
|
||||
float theta_dot = - leg_r->legState.phi0_dot - body_phi_dot;
|
||||
|
||||
float theta_dot = - balanceControl->leg_right.legState.phi0_dot - body_phi_dot;
|
||||
//计算机体速度
|
||||
float xb = x + 2 * leg_r->legState.L0 * arm_sin_f32(theta);
|
||||
float xb_dot = x_dot + 2*(leg_r->legState.L0_dot* arm_sin_f32(theta) + leg_r->legState.L0 * theta_dot * arm_cos_f32(theta));
|
||||
|
||||
estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[0]);
|
||||
|
||||
balanceControl->state[0] = theta;
|
||||
balanceControl->state[1] = theta_dot;
|
||||
balanceControl->state[2] = x; balanceControl->state[3] = x_dot;
|
||||
balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot;
|
||||
balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] +
|
||||
0.75f * theta_dot;
|
||||
//balanceControl->state[1] = theta_dot;
|
||||
balanceControl->state[2] = x;
|
||||
balanceControl->state[3] = x_dot;
|
||||
|
||||
balanceControl->state[4] = body_phi;
|
||||
balanceControl->state[5] = (1.0f - 0.75f) * balanceControl->state[5] +
|
||||
0.75f * body_phi_dot;
|
||||
// balanceControl->state[5] = body_phi_dot;
|
||||
|
||||
//Observer_Estimate(&balanceControl->state_observer,balanceControl->state,balanceControl->balance_LQR.control_vector,L_average);
|
||||
|
||||
//地面支持力解算
|
||||
//balanceControl->FN_right = calculate_F(MotionAccel[2],theta,theta_dot,leg_r->legState.L0,leg_r->legState.L0_dot,leg_r->legFeedback.F,leg_r->legFeedback.Tp);
|
||||
//balanceControl->FN_left = calculate_F(MotionAccel[2],theta,theta_dot,leg_l->legState.L0,leg_l->legState.L0_dot,leg_l->legFeedback.F,leg_l->legFeedback.Tp);
|
||||
|
||||
mylqr_k(L_average,K_matrix);
|
||||
LQR_set_K(&balanceControl->balance_LQR,K_matrix);
|
||||
|
|
@ -68,7 +92,7 @@ void Balance_Control_Loop(Balance_Control_t* balanceControl)
|
|||
float F_pid_out_l = PIDCalculate(&leg_l->Length_PID,leg_l->legState.L0,leg_l->target_L0);
|
||||
//控制平均腿长
|
||||
float average_length = (leg_r->legState.L0 + leg_l->legState.L0)/2;
|
||||
float F_pid_out= PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
|
||||
float F_pid_out = PIDCalculate(&balanceControl->leg_length_PID,average_length,balanceControl->target_L0);
|
||||
//float F_pid_out_l = PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
|
||||
|
||||
float Tp_pid_out_r = PIDCalculate(&leg_r->Phi0_PID,leg_r->legState.phi0,leg_r->target_phi0);
|
||||
|
|
@ -78,8 +102,8 @@ void Balance_Control_Loop(Balance_Control_t* balanceControl)
|
|||
|
||||
float cor_Tp = PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
|
||||
|
||||
Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 20,Tp + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 20,Tp - cor_Tp);
|
||||
Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 30,Tp + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 30,Tp - cor_Tp);
|
||||
|
||||
// Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
|
||||
// Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp);
|
||||
|
|
@ -88,9 +112,22 @@ void Balance_Control_Loop(Balance_Control_t* balanceControl)
|
|||
VMC_getT(leg_l);
|
||||
}
|
||||
|
||||
void target_x_set(Balance_Control_t* balanceControl,float x)
|
||||
void target_x_set(Balance_Control_t* balanceControl,float add_x)
|
||||
{
|
||||
balanceControl->target_state [2] = x;
|
||||
balanceControl->target_state [2] += add_x;
|
||||
}
|
||||
|
||||
void state_clear(Balance_Control_t* balanceControl)
|
||||
{
|
||||
//清除位移状态量
|
||||
balanceControl->target_state [2] = 0;
|
||||
balanceControl->state[2] = 0;
|
||||
}
|
||||
|
||||
void target_L_set(Balance_Control_t* balanceControl,float add_L)
|
||||
{
|
||||
balanceControl->target_L0 += add_L;
|
||||
float_constrain(balanceControl->target_L0,0.15f,0.30f);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -103,47 +140,47 @@ void target_x_set(Balance_Control_t* balanceControl,float x)
|
|||
* float K[12]
|
||||
* Return Type : void
|
||||
*/
|
||||
void mylqr_k(float L0, float K[12])
|
||||
void mylqr_k(float L0, float K_mat[12])
|
||||
{
|
||||
float t2;
|
||||
float t3;
|
||||
|
||||
float K_temp[12];
|
||||
float K[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
t2 = L0 * L0;
|
||||
t3 = t2 * L0;
|
||||
/* 06-Mar-2024 21:40:29 */
|
||||
K_temp[0] =
|
||||
((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F;
|
||||
K_temp[1] =
|
||||
((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F;
|
||||
K_temp[2] =
|
||||
((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F;
|
||||
K_temp[3] =
|
||||
((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 * -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 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F;
|
||||
K_temp[8] =
|
||||
((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F;
|
||||
K_temp[9] =
|
||||
((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 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F;
|
||||
|
||||
/* 11-Mar-2024 19:12:17 */
|
||||
K[0] =
|
||||
((L0 * -173.845428F + t2 * 178.098602F) - t3 * 104.773026F) - 8.81736755F;
|
||||
K[1] =
|
||||
((L0 * 150.104736F - t2 * 511.085876F) + t3 * 523.35144F) + 10.6000547F;
|
||||
K[2] =
|
||||
((L0 * -16.623167F - t2 * 12.9591064F) + t3 * 11.4631195F) - 2.62765169F;
|
||||
K[3] =
|
||||
((L0 * -1.31775725F - t2 * 24.5691776F) + t3 * 32.5003357F) + 5.97731924F;
|
||||
K[4] =
|
||||
((L0 * 4.03619576F - t2 * 33.4860535F) + t3 * 47.0306854F) - 11.8519392F;
|
||||
K[5] =
|
||||
((L0 * 23.7967739F - t2 * 163.993057F) + t3 * 207.196823F) + 8.34590435F;
|
||||
K[6] =
|
||||
((L0 * 29.8367863F - t2 * 116.608421F) + t3 * 131.16246F) - 16.6295223F;
|
||||
K[7] =
|
||||
((L0 * -16.2756023F - t2 * 56.7522507F) + t3 * 104.046684F) + 14.3186169F;
|
||||
K[8] =
|
||||
((L0 * -104.948708F + t2 * 87.532341F) - t3 * 7.19816732F) + 51.9084816F;
|
||||
K[9] =
|
||||
((L0 * 172.631012F - t2 * 274.727661F) + t3 * 150.935028F) + 42.625248F;
|
||||
K[10] =
|
||||
((L0 * -11.3688688F + t2 * 19.2530308F) - t3 * 15.6853495F) + 6.75911F;
|
||||
K[11] =
|
||||
((L0 * 10.4498978F - t2 * 4.97098875F) - t3 * 7.18331099F) + 2.7609446F;
|
||||
//matlab赋值顺序不同
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[i] = K_temp[2*i];
|
||||
K_mat[i] = K[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[6+i] = K_temp[2*i+1];
|
||||
K_mat[6+i] = K[2*i+1];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@
|
|||
#include "LQR_old.h"
|
||||
#include "controller.h"
|
||||
#include "kalman_filter.h"
|
||||
#include "estimator.h"
|
||||
#include "observer.h"
|
||||
typedef struct
|
||||
{
|
||||
LegInstance leg_right;
|
||||
|
|
@ -19,9 +21,13 @@ typedef struct
|
|||
|
||||
float state[6];
|
||||
float target_state[6];
|
||||
float target_L0; //平均腿长目标值
|
||||
float FN_left; //地面支持力
|
||||
float FN_right;
|
||||
|
||||
LQRInstance balance_LQR;
|
||||
|
||||
estimator_t v_estimator;
|
||||
BalanceObserver state_observer;
|
||||
|
||||
}Balance_Control_t;
|
||||
|
||||
|
|
@ -36,8 +42,10 @@ typedef struct
|
|||
|
||||
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig);
|
||||
//void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float body_phi,float body_phi_dot,float x,float x_dot);
|
||||
void Balance_feedback_update(Balance_Control_t* balanceControl,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 Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,const float MotionAccel[3]);
|
||||
void Balance_Control_Loop(Balance_Control_t* balanceControl);
|
||||
void target_x_set(Balance_Control_t* balanceControl,float x);
|
||||
void target_x_set(Balance_Control_t* balanceControl,float add_x);
|
||||
void target_L_set(Balance_Control_t* balanceControl,float add_L);
|
||||
void state_clear(Balance_Control_t* balanceControl);
|
||||
|
||||
#endif //WHEEL_LEGGED_BALANCE_OLD_H
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#include "balance_old.h"
|
||||
#include "observer.h"
|
||||
#include "estimator.h"
|
||||
|
||||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
|
|
@ -91,11 +92,21 @@ void ChassisInit()
|
|||
|
||||
Motor_Init_Config_s wheel_motor_config = {
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config ={
|
||||
.speed_PID = {
|
||||
.Kp = -100, // 50
|
||||
.Ki = 0, // 200
|
||||
.Kd = 0,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 3000,
|
||||
.MaxOut = 20000,
|
||||
},
|
||||
},
|
||||
.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,
|
||||
.close_loop_type = OPEN_LOOP,
|
||||
},
|
||||
.motor_type = LK9025,
|
||||
};
|
||||
|
|
@ -172,9 +183,9 @@ void ChassisInit()
|
|||
.F_feedforward = 0,
|
||||
},
|
||||
.leg_cor_PID_config = {
|
||||
.Kp = 25,
|
||||
.Kp = 25.0f,//25,//25,
|
||||
.Ki = 0,
|
||||
.Kd = 3,
|
||||
.Kd = 3.0f,//3,
|
||||
.MaxOut = 3,
|
||||
.Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 0.01f,
|
||||
|
|
@ -189,7 +200,7 @@ void ChassisInit()
|
|||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||||
//转向环
|
||||
PID_Init_Config_s turn_pid_cfg = {
|
||||
.Kp = 10.0f,//1,
|
||||
.Kp = 10.0f,//10.0f,//1,
|
||||
.Ki = 0,
|
||||
.Kd = 0.0f,
|
||||
.MaxOut = 2,
|
||||
|
|
@ -261,8 +272,6 @@ static void Chassis_State_update()
|
|||
float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
|
||||
float body_phi_dot = Chassis_IMU_data->Gyro[X];
|
||||
|
||||
float total_angle = (-wheel_motor_r->measure.total_angle + wheel_motor_l->measure.total_angle)/2;
|
||||
|
||||
//陀螺仪积分获取位移和速度
|
||||
static uint32_t integral_cnt = 0;
|
||||
static float imu_v,imu_x;
|
||||
|
|
@ -277,18 +286,32 @@ static void Chassis_State_update()
|
|||
|
||||
|
||||
//float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
|
||||
static float left_offset = 0;
|
||||
static float right_offset = 0;
|
||||
//倒地需要清零总位移数据
|
||||
if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
left_offset = wheel_motor_l->measure.total_angle;
|
||||
right_offset = wheel_motor_r->measure.total_angle;
|
||||
}
|
||||
|
||||
float x_l = wheel_motor_l->measure.total_angle - left_offset;
|
||||
float x_r = wheel_motor_r->measure.total_angle - right_offset;
|
||||
//驱动轮位移与速度
|
||||
// float total_angle = (- wheel_motor_r->measure.total_angle + wheel_motor_l->measure.total_angle)/2;
|
||||
// float x = (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
|
||||
// float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
|
||||
// float x_dot = speed * RADIUS_WHEEL;
|
||||
|
||||
float x = (total_angle * DEGREE_2_RAD ) * 0.135f/2;
|
||||
|
||||
float total_angle = (- x_r + x_l)/2;
|
||||
float x = (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
|
||||
float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
|
||||
float x_dot = speed * RADIUS_WHEEL;
|
||||
|
||||
float x_dot = speed * 0.135f/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_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_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
|
||||
|
||||
if( ChassisState == FAIL )
|
||||
{
|
||||
|
|
@ -310,7 +333,7 @@ void ChassisTask()
|
|||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_NO_FOLLOW)
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
HTMotorStop(motor_lf);
|
||||
HTMotorStop(motor_rf);
|
||||
|
|
@ -318,6 +341,14 @@ void ChassisTask()
|
|||
HTMotorStop(motor_rb);
|
||||
LKMotorStop(wheel_motor_r);
|
||||
LKMotorStop(wheel_motor_l);
|
||||
|
||||
//清除相关pid控制量
|
||||
PIDClear(&BalanceControl.leg_length_PID);
|
||||
PIDClear(&Turn_Loop_PID);
|
||||
PIDClear(&Roll_Loop_PID);
|
||||
PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
//清除状态量
|
||||
state_clear(&BalanceControl);
|
||||
}
|
||||
else
|
||||
{ // 正常工作
|
||||
|
|
@ -339,10 +370,11 @@ void ChassisTask()
|
|||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
|
||||
//chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
chassis_cmd_recv.wz = 0.01f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
|
||||
break;
|
||||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
//chassis_cmd_recv.wz = 4000;
|
||||
chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
|
||||
chassis_cmd_recv.wz = 2.0f;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -367,74 +399,61 @@ void ChassisTask()
|
|||
//获取数据并计算状态量
|
||||
Chassis_State_update();
|
||||
|
||||
float vofa_send_data[8];
|
||||
float vofa_send_data[12];
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
|
||||
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);
|
||||
// for(int i = 0; i<6;i++)
|
||||
// vofa_send_data[i] = BalanceControl.state[i];
|
||||
// for(int i = 0; i<6;i++)
|
||||
// vofa_send_data[i+6] = BalanceControl.state_observer.Estimate_X[i];
|
||||
vofa_justfloat_output(vofa_send_data,12*4,&huart1);
|
||||
|
||||
static float target_x = 0;
|
||||
static float target_yaw = 0;
|
||||
if(chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 右侧开关状态[下],底盘跟随云台
|
||||
if(chassis_cmd_recv.chassis_mode != CHASSIS_ZERO_FORCE) // 右侧开关状态[下],底盘跟随云台
|
||||
{
|
||||
float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
|
||||
//将roll轴自平衡PID输出直接叠加到前馈支持力上
|
||||
BalanceControl.leg_right.F_feedforward = Roll_pid_out;
|
||||
BalanceControl.leg_left.F_feedforward = -Roll_pid_out;
|
||||
|
||||
target_x += chassis_cmd_recv.vx/5280 * 0.003f;
|
||||
target_x_set(&BalanceControl,target_x);
|
||||
|
||||
//target_x += chassis_cmd_recv.vx/5280 * 0.003f;
|
||||
target_x_set(&BalanceControl,chassis_cmd_recv.vx/5280 * 0.003f);
|
||||
target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
|
||||
Balance_Control_Loop(&BalanceControl);
|
||||
|
||||
static float wz_feedback;
|
||||
wz_feedback = (1.0f - 0.75f) * wz_feedback + 0.75f * Chassis_IMU_data->Gyro[Z];
|
||||
float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
|
||||
|
||||
static float T,wheel_current_r,wheel_current_l;
|
||||
|
||||
target_yaw = 0.0f;//chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z]
|
||||
T = BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
|
||||
|
||||
//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);
|
||||
float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],target_yaw);
|
||||
//T = (1.0f - 0.75f) * T + 0.75f * BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
|
||||
|
||||
float T = float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
|
||||
// wheel_current_r = (1.0f - 0.75f) * wheel_current_r + 0.75f * (T+turn_T) * LK_TORQUE_2_CMD;//+turn_T
|
||||
// wheel_current_l = (1.0f - 0.75f) * wheel_current_l + 0.75f * (T-turn_T) * LK_TORQUE_2_CMD;//-turn_T
|
||||
|
||||
float wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
|
||||
float wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;
|
||||
wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
|
||||
wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;
|
||||
|
||||
float_constrain(wheel_current_r,-2000,2000);
|
||||
float_constrain(wheel_current_l,-2000,2000);
|
||||
float_constrain(wheel_current_r,-4000,4000);
|
||||
float_constrain(wheel_current_l,-4000,4000);
|
||||
|
||||
if (ChassisState == FAIL) // 倒地 未起身
|
||||
{
|
||||
HTMotorSetRef(motor_rf,0);
|
||||
HTMotorSetRef(motor_rb,0);
|
||||
|
||||
HTMotorSetRef(motor_lf,0);
|
||||
HTMotorSetRef(motor_lb,0);
|
||||
|
||||
PIDClear(&BalanceControl.leg_length_PID);
|
||||
PIDClear(&Turn_Loop_PID);
|
||||
PIDClear(&Roll_Loop_PID);
|
||||
PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
}
|
||||
else
|
||||
// if (ChassisState == FAIL) // 倒地 未起身
|
||||
// {
|
||||
// HTMotorSetRef(motor_rf,0);
|
||||
// HTMotorSetRef(motor_rb,0);
|
||||
//
|
||||
// HTMotorSetRef(motor_lf,0);
|
||||
// HTMotorSetRef(motor_lb,0);
|
||||
//
|
||||
// PIDClear(&BalanceControl.leg_length_PID);
|
||||
// PIDClear(&Turn_Loop_PID);
|
||||
// PIDClear(&Roll_Loop_PID);
|
||||
// PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
// }
|
||||
// else
|
||||
{
|
||||
HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
|
||||
HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
|
||||
|
|
@ -450,12 +469,12 @@ void ChassisTask()
|
|||
|
||||
LKMotorSetRef(wheel_motor_r,wheel_current_r);
|
||||
LKMotorSetRef(wheel_motor_l,wheel_current_l);
|
||||
// LKMotorSetRef(wheel_motor_r,100);
|
||||
// LKMotorSetRef(wheel_motor_l,100);
|
||||
// LKMotorSetRef(wheel_motor_r,0);
|
||||
// LKMotorSetRef(wheel_motor_l,0);
|
||||
|
||||
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);
|
||||
Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current/HT_CMD_COEF,motor_lb->measure.real_current/HT_CMD_COEF);
|
||||
// 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);
|
||||
// Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current/HT_CMD_COEF,motor_lb->measure.real_current/HT_CMD_COEF);
|
||||
}
|
||||
else{
|
||||
PIDClear(&BalanceControl.leg_length_PID);
|
||||
|
|
|
|||
|
|
@ -54,12 +54,14 @@ void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi
|
|||
|
||||
|
||||
//对phi0求导
|
||||
// leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
|
||||
// if(leg_state->last_phi0>=0)
|
||||
// leg_state->phi0_dot = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
|
||||
// else
|
||||
// leg_state->phi0_dot = 0;
|
||||
// leg_state->last_phi0 = leg_state->phi0;
|
||||
leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
|
||||
//if(leg_state->last_phi0>=0)
|
||||
leg_state->phi0_dot_num = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
|
||||
//else
|
||||
//leg_state->phi0_dot_num = 0;
|
||||
leg_state->last_phi0 = leg_state->phi0;
|
||||
|
||||
|
||||
leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
|
||||
leg_state->L0_dot = get_dL0(phi1,phi4,phi1_dot,phi4_dot);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,8 +21,10 @@ typedef struct
|
|||
float dt; //用于计算时间并求导
|
||||
float last_phi0;
|
||||
float phi0_dot;
|
||||
float phi0_dot_num;
|
||||
|
||||
float L0_dot;
|
||||
float L0_dot_num;
|
||||
|
||||
}Leg_State_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -7,22 +7,23 @@
|
|||
#include "arm_math.h"
|
||||
|
||||
static const float dt = 1e-3;
|
||||
|
||||
static const float p_A[6][3] = {
|
||||
{110.149443140716,-209.796047546046, 108.212820026855},
|
||||
{6.56756328149907,-12.5089040777703,6.45209383844021},
|
||||
{20.8432584790045,-14.5681682729471,-0.265391946757807},
|
||||
{1.24276088149285,-0.868614169078260,0.164688375466239},
|
||||
{-121.880643460273,85.1871469584697,1.55187545520287},
|
||||
{-7.26702574149722,5.07920841420309,16.7402602760541}
|
||||
{-2089.29388440040,913.777119858069,161.725026756580},
|
||||
{-48.4503739138708,21.1903377794882,3.75037618024515},
|
||||
{349.621231737198,-308.272343710195,14.5058784104114},
|
||||
{8.10765758344078,-7.14878382193259,1.73804857662225},
|
||||
{-126.242793963670,111.312353023678,-5.23784728498450},
|
||||
{-2.92754916727612,2.58131475207839,21.1973941504635}
|
||||
};
|
||||
|
||||
static const float p_B[6][3] = {
|
||||
{-171.924494208671,125.455249123645,-27.5944606199982},
|
||||
{169.057788940511,-119.995181259560,24.7781571289566},
|
||||
{-0.576505386985648,0.0174968125883799,0.781810074568448},
|
||||
{0.0340470941093140,0.361648497135550,-0.267186973741535},
|
||||
{3.37110666237256,-0.102312350965865,-1.14202204587003},
|
||||
{-0.199089875614915,-2.11473419963208,6.60130194247413}
|
||||
{-5.36398695946053,59.5647709213161,-40.3119963098152},
|
||||
{127.834870314177,-113.128844359679,30.8319480335393},
|
||||
{-33.9678204232552,24.3044065007359,0.798747072246912},
|
||||
{13.4736135478509,-6.23400169559697,-0.818937686872829},
|
||||
{12.2652521237935,-8.77594351760660,1.29067140754752},
|
||||
{-4.86511248363210,2.25100114119497,2.36237087051996},
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -44,19 +45,47 @@ const float gEstimateKF_H[36] = {1, 0, 0, 0, 0,0,
|
|||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1}; // 由于不需要异步量测自适应，这里直接设置矩阵H为常量
|
||||
|
||||
|
||||
//后验估计协方差初始值
|
||||
static float P[36] = {1, 0, 0, 0, 0,0,
|
||||
0, 1, 0, 0, 0,0,
|
||||
0, 0, 1, 0, 0,0,
|
||||
0, 0, 0, 1, 0,0,
|
||||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1};
|
||||
// R Q矩阵初始值（其实这里设置多少都无所谓）
|
||||
static float Q[36] = {1, 0, 0, 0, 0,0,
|
||||
0, 1, 0, 0, 0,0,
|
||||
0, 0, 1, 0, 0,0,
|
||||
0, 0, 0, 1, 0,0,
|
||||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1};
|
||||
static float R[36] = {1, 0, 0, 0, 0,0,
|
||||
0, 1, 0, 0, 0,0,
|
||||
0, 0, 1, 0, 0,0,
|
||||
0, 0, 0, 1, 0,0,
|
||||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1};
|
||||
void Observer_Init(BalanceObserver* observer)
|
||||
{
|
||||
KalmanFilter_t *kf = &observer->stateEstimateKF;
|
||||
Kalman_Filter_Init(kf,6,2,6);
|
||||
|
||||
for (uint8_t i = 0; i < 36; i += 7)
|
||||
{
|
||||
// 初始化过程噪声与量测噪声
|
||||
Q[i] = 1.0f;//process_noise;
|
||||
R[i] = 0.01f;//measure_noise;
|
||||
}
|
||||
|
||||
//暂时只使用状态预测一个步骤
|
||||
kf->UseAutoAdjustment = 0;
|
||||
kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
|
||||
//kf->UseAutoAdjustment = 1;
|
||||
//kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
|
||||
|
||||
memcpy(kf->F_data , gEstimateKF_F , sizeof(gEstimateKF_F));
|
||||
memcpy(kf->B_data , gEstimateKF_B , sizeof(gEstimateKF_B));
|
||||
memcpy(kf->H_data , gEstimateKF_H , sizeof(gEstimateKF_H));
|
||||
memcpy(kf->Q_data , Q,sizeof(Q));
|
||||
memcpy(kf->R_data , R,sizeof(R));
|
||||
// set rest of memory to 0
|
||||
DWT_GetDeltaT(&observer->DWT_CNT);
|
||||
}
|
||||
|
|
@ -82,11 +111,22 @@ void Observer_Estimate(BalanceObserver* observer,float* zk,float* uk,float L0)
|
|||
kf->B_data[6] = B[2]; kf->B_data[7] = B[3];
|
||||
kf->B_data[10] = B[4]; kf->B_data[11] = B[5];
|
||||
//测量值更新
|
||||
memcpy(kf->MeasuredVector,zk,6*4 );
|
||||
//测量值更新
|
||||
memcpy(kf->ControlVector,uk,2*4);
|
||||
// memcpy(kf->MeasuredVector,zk,6*4);
|
||||
// //测量值更新
|
||||
// memcpy(kf->ControlVector,uk,2*4);
|
||||
|
||||
for(uint8_t i = 0; i < 6; i++)
|
||||
kf->MeasuredVector[i] = zk[i];
|
||||
for(uint8_t i = 0; i < 2; i++)
|
||||
kf->ControlVector[i] = uk[i];
|
||||
|
||||
Kalman_Filter_Update(kf);
|
||||
|
||||
memcpy(observer->Estimate_X,kf->xhatminus_data,6*4);
|
||||
// 提取估计值
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
observer->Estimate_X[i] = kf->FilteredValue[i];
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -73,7 +73,7 @@ void RobotCMDInit()
|
|||
#endif // GIMBAL_BOARD
|
||||
gimbal_cmd_send.pitch = 0;
|
||||
|
||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
robot_state = ROBOT_STOP; // 轮腿 上电后进入ROBOT_STOP 保证安全(对应倒地状态）
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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 // 使用虚拟串口发送视觉数据
|
||||
|
|
@ -40,7 +40,7 @@
|
|||
#define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向)
|
||||
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
|
||||
#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
|
||||
#define RADIUS_WHEEL 0.06f // 轮子半径
|
||||
#define RADIUS_WHEEL 0.1f // 轮子半径
|
||||
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
|
||||
|
||||
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
|
||||
|
|
|
|||
|
|
@ -143,18 +143,18 @@ void LKMotorControl()
|
|||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
if (setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->speed_rads;
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
}
|
||||
|
||||
if (setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
//pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
set = (int16_t)pid_ref;
|
||||
|
|
|
|||
Loading…
Reference in New Issue