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

底盘状态机完善

This commit is contained in:
宋家齐 2024-03-17 17:51:24 +08:00
parent eccdd5be2e
commit 23feabe1fc
13 changed files with 321 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -35,6 +35,7 @@ add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-le
# Enable assembler files preprocessing # Enable assembler files preprocessing
add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>) add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>)
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed") message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast) add_compile_options(-Ofast)

56
application/balance/estimator.c Normal file
View File

@ -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];
}
}

62
application/balance/estimator.cpp
View File

@ -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];
}

14
application/balance/estimator.h
View File

@ -6,19 +6,15 @@
#ifndef WHEEL_LEGGED_ESTIMATOR_H #ifndef WHEEL_LEGGED_ESTIMATOR_H
#define WHEEL_LEGGED_ESTIMATOR_H #define WHEEL_LEGGED_ESTIMATOR_H
typedef struct
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作为状态观测器 KalmanFilter_t EstimateKF_; //使用KF作为状态观测器
float Estimate_X_[2]; //观测器估计的状态量 float Estimate_X_[2]; //观测器估计的状态量
uint32_t DWT_CNT_; //计时用 uint32_t DWT_CNT_; //计时用
float dt_; 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 #endif //WHEEL_LEGGED_ESTIMATOR_H

131
application/chassis/balance_old.c
View File

@ -4,10 +4,12 @@
#include "balance_old.h" #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) 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_right,&InitConfig.legInitConfig);
Leg_Init(&balanceControl->leg_left,&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_coordination_PID,&InitConfig.leg_cor_PID_config);
PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf); 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->state,0,6);
memset(balanceControl->target_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); 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} // 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]); LegInstance *leg_r = &balanceControl->leg_right;
Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]); 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 K_matrix[12]={0};
float L_average = (balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2; float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
//(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
float alpha = PI/2 - balanceControl->leg_right.legState.phi0; float alpha = PI/2 - leg_r->legState.phi0;
float theta = alpha - body_phi; float theta = alpha - body_phi;
// static uint32_t dot_cnt = 0; // static uint32_t dot_cnt = 0;
// float dot_dt = DWT_GetDeltaT(&dot_cnt); // float dot_dt = DWT_GetDeltaT(&dot_cnt);
// float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt; // 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[0] = theta;
balanceControl->state[1] = theta_dot; balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] +
balanceControl->state[2] = x; balanceControl->state[3] = x_dot; 0.75f * theta_dot;
balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_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); mylqr_k(L_average,K_matrix);
LQR_set_K(&balanceControl->balance_LQR,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 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 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 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); 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); 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_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 + 20,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_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
// Leg_Input_update(&leg_l->legInput,F_pid_out_l + 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); 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] * float K[12]
* Return Type : void * Return Type : void
*/ */
void mylqr_k(float L0, float K[12]) void mylqr_k(float L0, float K_mat[12])
{ {
float t2; float t2;
float t3; float t3;
float K_temp[12]; float K[12];
/* This function was generated by the Symbolic Math Toolbox version 9.2. /* This function was generated by the Symbolic Math Toolbox version 9.2.
*/ */
t2 = L0 * L0; t2 = L0 * L0;
t3 = t2 * L0; t3 = t2 * L0;
/* 06-Mar-2024 21:40:29 */ /* 11-Mar-2024 19:12:17 */
K_temp[0] = K[0] =
((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F; ((L0 * -173.845428F + t2 * 178.098602F) - t3 * 104.773026F) - 8.81736755F;
K_temp[1] = K[1] =
((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F; ((L0 * 150.104736F - t2 * 511.085876F) + t3 * 523.35144F) + 10.6000547F;
K_temp[2] = K[2] =
((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F; ((L0 * -16.623167F - t2 * 12.9591064F) + t3 * 11.4631195F) - 2.62765169F;
K_temp[3] = K[3] =
((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F; ((L0 * -1.31775725F - t2 * 24.5691776F) + t3 * 32.5003357F) + 5.97731924F;
K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F; K[4] =
K_temp[5] = ((L0 * 4.03619576F - t2 * 33.4860535F) + t3 * 47.0306854F) - 11.8519392F;
((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F; K[5] =
K_temp[6] = ((L0 * 23.7967739F - t2 * 163.993057F) + t3 * 207.196823F) + 8.34590435F;
((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F; K[6] =
K_temp[7] = ((L0 * 29.8367863F - t2 * 116.608421F) + t3 * 131.16246F) - 16.6295223F;
((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F; K[7] =
K_temp[8] = ((L0 * -16.2756023F - t2 * 56.7522507F) + t3 * 104.046684F) + 14.3186169F;
((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F; K[8] =
K_temp[9] = ((L0 * -104.948708F + t2 * 87.532341F) - t3 * 7.19816732F) + 51.9084816F;
((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F; K[9] =
K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) + ((L0 * 172.631012F - t2 * 274.727661F) + t3 * 150.935028F) + 42.625248F;
5.52106953F; K[10] =
K_temp[11] = ((L0 * -11.3688688F + t2 * 19.2530308F) - t3 * 15.6853495F) + 6.75911F;
((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F; K[11] =
((L0 * 10.4498978F - t2 * 4.97098875F) - t3 * 7.18331099F) + 2.7609446F;
//matlab赋值顺序不同 //matlab赋值顺序不同
for (int i = 0; i < 6; ++i) { 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) { for (int i = 0; i < 6; ++i) {
K[6+i] = K_temp[2*i+1]; K_mat[6+i] = K[2*i+1];
} }
} }

14
application/chassis/balance_old.h
View File

@ -9,6 +9,8 @@
#include "LQR_old.h" #include "LQR_old.h"
#include "controller.h" #include "controller.h"
#include "kalman_filter.h" #include "kalman_filter.h"
#include "estimator.h"
#include "observer.h"
typedef struct typedef struct
{ {
LegInstance leg_right; LegInstance leg_right;
@ -19,9 +21,13 @@ typedef struct
float state[6]; float state[6];
float target_state[6]; float target_state[6];
float target_L0; //平均腿长目标值
float FN_left; //地面支持力
float FN_right;
LQRInstance balance_LQR; LQRInstance balance_LQR;
estimator_t v_estimator;
BalanceObserver state_observer;
}Balance_Control_t; }Balance_Control_t;
@ -36,8 +42,10 @@ typedef struct
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig); 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 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 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 #endif //WHEEL_LEGGED_BALANCE_OLD_H

157
application/chassis/chassis.c
View File

@ -24,6 +24,7 @@
#include "balance_old.h" #include "balance_old.h"
#include "observer.h" #include "observer.h"
#include "estimator.h"
#include "general_def.h" #include "general_def.h"
#include "bsp_dwt.h" #include "bsp_dwt.h"
@ -91,11 +92,21 @@ void ChassisInit()
Motor_Init_Config_s wheel_motor_config = { Motor_Init_Config_s wheel_motor_config = {
.can_init_config.can_handle = &hcan1, .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 = { .controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = OPEN_LOOP, .outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,//| CURRENT_LOOP, .close_loop_type = OPEN_LOOP,
}, },
.motor_type = LK9025, .motor_type = LK9025,
}; };
@ -172,9 +183,9 @@ void ChassisInit()
.F_feedforward = 0, .F_feedforward = 0,
}, },
.leg_cor_PID_config = { .leg_cor_PID_config = {
.Kp = 25, .Kp = 25.0f,//25,//25,
.Ki = 0, .Ki = 0,
.Kd = 3, .Kd = 3.0f,//3,
.MaxOut = 3, .MaxOut = 3,
.Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter, .Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter,
.Derivative_LPF_RC = 0.01f, .Derivative_LPF_RC = 0.01f,
@ -189,7 +200,7 @@ void ChassisInit()
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
//转向环 //转向环
PID_Init_Config_s turn_pid_cfg = { PID_Init_Config_s turn_pid_cfg = {
.Kp = 10.0f,//1, .Kp = 10.0f,//10.0f,//1,
.Ki = 0, .Ki = 0,
.Kd = 0.0f, .Kd = 0.0f,
.MaxOut = 2, .MaxOut = 2,
@ -261,8 +272,6 @@ static void Chassis_State_update()
float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD; float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
float body_phi_dot = Chassis_IMU_data->Gyro[X]; 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 uint32_t integral_cnt = 0;
static float imu_v,imu_x; static float imu_v,imu_x;
@ -277,18 +286,32 @@ static void Chassis_State_update()
//float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt; //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 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]); Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
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);
if( ChassisState == FAIL ) if( ChassisState == FAIL )
{ {
@ -310,7 +333,7 @@ void ChassisTask()
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm); chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD #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_lf);
HTMotorStop(motor_rf); HTMotorStop(motor_rf);
@ -318,6 +341,14 @@ void ChassisTask()
HTMotorStop(motor_rb); HTMotorStop(motor_rb);
LKMotorStop(wheel_motor_r); LKMotorStop(wheel_motor_r);
LKMotorStop(wheel_motor_l); 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 else
{ // 正常工作 { // 正常工作
@ -339,10 +370,11 @@ void ChassisTask()
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 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 = 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; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
//chassis_cmd_recv.wz = 4000; //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; break;
default: default:
break; break;
@ -367,74 +399,61 @@ void ChassisTask()
//获取数据并计算状态量 //获取数据并计算状态量
Chassis_State_update(); 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; // for(int i = 0; i<6;i++)
BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f); // vofa_send_data[i] = BalanceControl.state[i];
BalanceControl.leg_right.target_phi0 = PI/2; // for(int i = 0; i<6;i++)
// vofa_send_data[i+6] = BalanceControl.state_observer.Estimate_X[i];
BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0; vofa_justfloat_output(vofa_send_data,12*4,&huart1);
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);
static float target_x = 0; static float target_x = 0;
static float target_yaw = 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); float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
//将roll轴自平衡PID输出直接叠加到前馈支持力上 //将roll轴自平衡PID输出直接叠加到前馈支持力上
BalanceControl.leg_right.F_feedforward = Roll_pid_out; BalanceControl.leg_right.F_feedforward = Roll_pid_out;
BalanceControl.leg_left.F_feedforward = -Roll_pid_out; BalanceControl.leg_left.F_feedforward = -Roll_pid_out;
target_x += chassis_cmd_recv.vx/5280 * 0.003f; //target_x += chassis_cmd_recv.vx/5280 * 0.003f;
target_x_set(&BalanceControl,target_x); 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); 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); //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 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);
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; wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
float wheel_current_l = (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_r,-4000,4000);
float_constrain(wheel_current_l,-2000,2000); float_constrain(wheel_current_l,-4000,4000);
if (ChassisState == FAIL) // 倒地 未起身 // if (ChassisState == FAIL) // 倒地 未起身
{ // {
HTMotorSetRef(motor_rf,0); // HTMotorSetRef(motor_rf,0);
HTMotorSetRef(motor_rb,0); // HTMotorSetRef(motor_rb,0);
//
HTMotorSetRef(motor_lf,0); // HTMotorSetRef(motor_lf,0);
HTMotorSetRef(motor_lb,0); // HTMotorSetRef(motor_lb,0);
//
PIDClear(&BalanceControl.leg_length_PID); // PIDClear(&BalanceControl.leg_length_PID);
PIDClear(&Turn_Loop_PID); // PIDClear(&Turn_Loop_PID);
PIDClear(&Roll_Loop_PID); // PIDClear(&Roll_Loop_PID);
PIDClear(&BalanceControl.leg_coordination_PID); // PIDClear(&BalanceControl.leg_coordination_PID);
} // }
else // else
{ {
HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF); HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * 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_r,wheel_current_r);
LKMotorSetRef(wheel_motor_l,wheel_current_l); LKMotorSetRef(wheel_motor_l,wheel_current_l);
// LKMotorSetRef(wheel_motor_r,100); // LKMotorSetRef(wheel_motor_r,0);
// LKMotorSetRef(wheel_motor_l,100); // LKMotorSetRef(wheel_motor_l,0);
Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0); // 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_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); // Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current/HT_CMD_COEF,motor_lb->measure.real_current/HT_CMD_COEF);
} }
else{ else{
PIDClear(&BalanceControl.leg_length_PID); PIDClear(&BalanceControl.leg_length_PID);

14
application/chassis/leg_old.c
View File

@ -54,12 +54,14 @@ void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi
//对phi0求导 //对phi0求导
// leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT); leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
// if(leg_state->last_phi0>=0) //if(leg_state->last_phi0>=0)
// leg_state->phi0_dot = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt; leg_state->phi0_dot_num = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
// else //else
// leg_state->phi0_dot = 0; //leg_state->phi0_dot_num = 0;
// leg_state->last_phi0 = leg_state->phi0; leg_state->last_phi0 = leg_state->phi0;
leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot); leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
leg_state->L0_dot = get_dL0(phi1,phi4,phi1_dot,phi4_dot); leg_state->L0_dot = get_dL0(phi1,phi4,phi1_dot,phi4_dot);
} }

2
application/chassis/leg_old.h
View File

@ -21,8 +21,10 @@ typedef struct
float dt; //用于计算时间并求导 float dt; //用于计算时间并求导
float last_phi0; float last_phi0;
float phi0_dot; float phi0_dot;
float phi0_dot_num;
float L0_dot; float L0_dot;
float L0_dot_num;
}Leg_State_t; }Leg_State_t;

78
application/chassis/observer.c
View File

@ -7,22 +7,23 @@
#include "arm_math.h" #include "arm_math.h"
static const float dt = 1e-3; static const float dt = 1e-3;
static const float p_A[6][3] = { static const float p_A[6][3] = {
{110.149443140716,-209.796047546046, 108.212820026855}, {-2089.29388440040,913.777119858069,161.725026756580},
{6.56756328149907,-12.5089040777703,6.45209383844021}, {-48.4503739138708,21.1903377794882,3.75037618024515},
{20.8432584790045,-14.5681682729471,-0.265391946757807}, {349.621231737198,-308.272343710195,14.5058784104114},
{1.24276088149285,-0.868614169078260,0.164688375466239}, {8.10765758344078,-7.14878382193259,1.73804857662225},
{-121.880643460273,85.1871469584697,1.55187545520287}, {-126.242793963670,111.312353023678,-5.23784728498450},
{-7.26702574149722,5.07920841420309,16.7402602760541} {-2.92754916727612,2.58131475207839,21.1973941504635}
}; };
static const float p_B[6][3] = { static const float p_B[6][3] = {
{-171.924494208671,125.455249123645,-27.5944606199982}, {-5.36398695946053,59.5647709213161,-40.3119963098152},
{169.057788940511,-119.995181259560,24.7781571289566}, {127.834870314177,-113.128844359679,30.8319480335393},
{-0.576505386985648,0.0174968125883799,0.781810074568448}, {-33.9678204232552,24.3044065007359,0.798747072246912},
{0.0340470941093140,0.361648497135550,-0.267186973741535}, {13.4736135478509,-6.23400169559697,-0.818937686872829},
{3.37110666237256,-0.102312350965865,-1.14202204587003}, {12.2652521237935,-8.77594351760660,1.29067140754752},
{-0.199089875614915,-2.11473419963208,6.60130194247413} {-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, 1,0,
0, 0, 0, 0, 0,1}; // 由于不需要异步量测自适应，这里直接设置矩阵H为常量 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) void Observer_Init(BalanceObserver* observer)
{ {
KalmanFilter_t *kf = &observer->stateEstimateKF; KalmanFilter_t *kf = &observer->stateEstimateKF;
Kalman_Filter_Init(kf,6,2,6); 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->UseAutoAdjustment = 1;
kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1; //kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
memcpy(kf->F_data , gEstimateKF_F , sizeof(gEstimateKF_F)); memcpy(kf->F_data , gEstimateKF_F , sizeof(gEstimateKF_F));
memcpy(kf->B_data , gEstimateKF_B , sizeof(gEstimateKF_B)); memcpy(kf->B_data , gEstimateKF_B , sizeof(gEstimateKF_B));
memcpy(kf->H_data , gEstimateKF_H , sizeof(gEstimateKF_H)); 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 // set rest of memory to 0
DWT_GetDeltaT(&observer->DWT_CNT); 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[6] = B[2]; kf->B_data[7] = B[3];
kf->B_data[10] = B[4]; kf->B_data[11] = B[5]; kf->B_data[10] = B[4]; kf->B_data[11] = B[5];
//测量值更新 //测量值更新
memcpy(kf->MeasuredVector,zk,6*4 ); // memcpy(kf->MeasuredVector,zk,6*4);
//测量值更新 // //测量值更新
memcpy(kf->ControlVector,uk,2*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); 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];
}
} }

2
application/cmd/robot_cmd.c
View File

@ -73,7 +73,7 @@ void RobotCMDInit()
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
gimbal_cmd_send.pitch = 0; gimbal_cmd_send.pitch = 0;
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 robot_state = ROBOT_STOP; // 轮腿 上电后进入ROBOT_STOP 保证安全(对应倒地状态）
} }
/** /**

6
application/robot_def.h
View File

@ -17,8 +17,8 @@
#include "stdint.h" #include "stdint.h"
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
#define ONE_BOARD // 单板控制整车 //#define ONE_BOARD // 单板控制整车
//#define CHASSIS_BOARD //底盘板 #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板 // #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据
@ -40,7 +40,7 @@
#define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向) #define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向)
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
#define CENTER_GIMBAL_OFFSET_Y 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 REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 #define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反

6
modules/motor/LKmotor/LK9025.c
View File

@ -143,18 +143,18 @@ void LKMotorControl()
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)) 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; pid_measure = *motor->other_speed_feedback_ptr;
else else
pid_measure = measure->speed_rads; 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) if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr; pid_ref += *motor->current_feedforward_ptr;
} }
if (setting->close_loop_type & CURRENT_LOOP) 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; set = (int16_t)pid_ref;