577 lines
23 KiB
C
577 lines
23 KiB
C
/**
|
||
* @file chassis.c
|
||
* @author NeoZeng neozng1@hnu.edu.cn
|
||
* @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
|
||
* 注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向
|
||
*
|
||
* @version 0.1
|
||
* @date 2022-12-04
|
||
*
|
||
* @copyright Copyright (c) 2022
|
||
*
|
||
*/
|
||
|
||
#include "chassis.h"
|
||
#include "robot_def.h"
|
||
#include "dji_motor.h"
|
||
#include "HT04.h"
|
||
#include "LK9025.h"
|
||
#include "super_cap.h"
|
||
#include "message_center.h"
|
||
#include "referee_task.h"
|
||
#include "power_meter.h"
|
||
#include "ins_task.h"
|
||
|
||
#include "balance.h"
|
||
#include "observer.h"
|
||
#include "estimator.h"
|
||
|
||
#include "general_def.h"
|
||
#include "bsp_dwt.h"
|
||
#include "referee_UI.h"
|
||
#include "arm_math.h"
|
||
#include "vofa.h"
|
||
#include "user_lib.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"
|
||
static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
|
||
INS_t *Chassis_IMU_data;
|
||
#endif // CHASSIS_BOARD
|
||
#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; // 底盘回传的反馈数据
|
||
|
||
static referee_info_t* referee_data; // 用于获取裁判系统的数据
|
||
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
|
||
|
||
static SuperCapInstance *cap; // 超级电容
|
||
static PowerMeterInstance *power_meter; //功率计
|
||
|
||
static HTMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
|
||
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;
|
||
|
||
BalanceObserver Observer;
|
||
|
||
PIDInstance Turn_Loop_PID , Roll_Loop_PID;
|
||
|
||
Chassis_state_e ChassisState,last_ChassisState;
|
||
chassis_mode_e last_chassis_mode;
|
||
|
||
first_order_filter_type_t wheel_r_filter,wheel_l_filter;
|
||
|
||
first_order_filter_type_t ht_rf_filter,ht_rb_filter,ht_lf_filter,ht_lb_filter;
|
||
first_order_filter_type_t vx_filter;
|
||
|
||
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,
|
||
},
|
||
.motor_type = HT04,
|
||
};
|
||
|
||
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,
|
||
},
|
||
.motor_type = LK9025,
|
||
};
|
||
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
||
chassis_motor_config.can_init_config.tx_id = 1;
|
||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||
motor_rf = HTMotorInit(&chassis_motor_config);
|
||
|
||
chassis_motor_config.can_init_config.tx_id = 2;
|
||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||
motor_rb = HTMotorInit(&chassis_motor_config);
|
||
|
||
|
||
chassis_motor_config.can_init_config.tx_id = 3;
|
||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||
motor_lf = HTMotorInit(&chassis_motor_config);
|
||
|
||
|
||
chassis_motor_config.can_init_config.tx_id = 4;
|
||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||
motor_lb = HTMotorInit(&chassis_motor_config);
|
||
|
||
//@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);
|
||
|
||
|
||
|
||
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
|
||
|
||
//超级电容
|
||
SuperCap_Init_Config_s cap_conf = {
|
||
.can_config = {
|
||
.can_handle = &hcan1,
|
||
.tx_id = 0x210,
|
||
.rx_id = 0x211,
|
||
}};
|
||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||
SuperCapSetPower(cap,70); // 超级电容限制功率
|
||
|
||
PowerMeter_Init_Config_s power_conf = {
|
||
.can_config = {
|
||
.can_handle = &hcan1,
|
||
.rx_id = 0x212,
|
||
}
|
||
};
|
||
//功率計初始化
|
||
power_meter = PowerMeterInit(&power_conf);
|
||
|
||
Balance_Init_config_s balanceInitConfig = {
|
||
.legInitConfig = {
|
||
.length_PID_conf = {
|
||
.Kp = 300,//500,//180 ,//50,
|
||
.Ki = 0,//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),
|
||
.IntegralLimit = 100,
|
||
},
|
||
.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),
|
||
.IntegralLimit = 100,
|
||
},
|
||
.init_target_L0 = 0.15f,
|
||
.F_feedforward = 0,
|
||
},
|
||
.leg_cor_PID_config = {
|
||
.Kp = 25.0f,//25,//25,
|
||
.Ki = 0,
|
||
.Kd = 3.0f,//3,
|
||
.MaxOut = 3,
|
||
.Improve = 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);
|
||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||
//转向环
|
||
PID_Init_Config_s turn_pid_cfg = {
|
||
.Kp = 12.0f,//10.0f,//1,
|
||
.Ki = 0,
|
||
.Kd = 0.8f,
|
||
.MaxOut = 2.0f,
|
||
|
||
.Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter,
|
||
.Derivative_LPF_RC = 0.001f
|
||
};
|
||
PIDInit(&Turn_Loop_PID,&turn_pid_cfg);
|
||
|
||
//Roll轴平衡
|
||
PID_Init_Config_s roll_pid_cfg = {
|
||
.Kp = 12.0f,
|
||
.Ki = 0,
|
||
.Kd = 0.5f,
|
||
.MaxOut = 100,
|
||
.Improve = PID_Derivative_On_Measurement,
|
||
};
|
||
PIDInit(&Roll_Loop_PID,&roll_pid_cfg);
|
||
|
||
Observer_Init(&Observer);
|
||
|
||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||
#ifdef CHASSIS_BOARD
|
||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||
|
||
CANComm_Init_Config_s comm_conf = {
|
||
.can_config = {
|
||
.can_handle = &hcan1,
|
||
.tx_id = 0x311,
|
||
.rx_id = 0x312,
|
||
},
|
||
.recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
|
||
.send_data_len = sizeof(Chassis_Upload_Data_s),
|
||
};
|
||
chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
|
||
#endif // CHASSIS_BOARD
|
||
|
||
#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
|
||
chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
|
||
chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
|
||
#endif // ONE_BOARD
|
||
const float a = 0.03f;
|
||
first_order_filter_init(&wheel_r_filter,1e-3f,&a);
|
||
first_order_filter_init(&wheel_l_filter,1e-3f,&a);
|
||
|
||
const float ht_a = 0.05f;
|
||
first_order_filter_init(&ht_rf_filter,1e-3f,&ht_a);
|
||
first_order_filter_init(&ht_rb_filter,1e-3f,&ht_a);
|
||
first_order_filter_init(&ht_lf_filter,1e-3f,&ht_a);
|
||
first_order_filter_init(&ht_lb_filter,1e-3f,&ht_a);
|
||
const float vx_a = 0.6f;
|
||
first_order_filter_init(&vx_filter,1e-3f,&vx_a);
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 获取数据并计算状态量
|
||
*
|
||
*/
|
||
|
||
#define HIP_OFFSET 0.49899f//0.23719f //0.119031f
|
||
static void Chassis_State_update()
|
||
{
|
||
float leg_phi[4]={0};
|
||
float leg_phi_dot[4] = {0};
|
||
leg_phi[0] = PI-(motor_rf->measure.total_angle - HIP_OFFSET);
|
||
leg_phi[1] = -(motor_rb->measure.total_angle + HIP_OFFSET);
|
||
leg_phi[2] = PI-(-motor_lf->measure.total_angle - HIP_OFFSET);
|
||
leg_phi[3] = -(-motor_lb->measure.total_angle + HIP_OFFSET);
|
||
|
||
//最高点上电
|
||
// leg_phi[0] = PI-(motor_rf->measure.total_angle + 0.81007f);
|
||
// leg_phi[1] = -(motor_rb->measure.total_angle - 0.81007f);
|
||
// leg_phi[2] = PI-(-motor_lf->measure.total_angle + 0.81007f);
|
||
// leg_phi[3] = -(-motor_lb->measure.total_angle - 0.81007f);
|
||
|
||
leg_phi_dot[0] = -motor_rf->measure.speed_rads;
|
||
leg_phi_dot[1] = -motor_rb->measure.speed_rads;
|
||
leg_phi_dot[2] = motor_lf->measure.speed_rads;
|
||
leg_phi_dot[3] = motor_lb->measure.speed_rads;
|
||
|
||
|
||
float body_phi = (Chassis_IMU_data->Pitch) * DEGREE_2_RAD; //+ 2
|
||
float body_phi_dot = Chassis_IMU_data->Gyro[X];
|
||
|
||
//陀螺仪积分获取位移和速度
|
||
static uint32_t integral_cnt = 0;
|
||
static float imu_v,imu_x;
|
||
float integral_dt = DWT_GetDeltaT(&integral_cnt);
|
||
|
||
if(fabsf(Chassis_IMU_data->MotionAccel_n[X])>=0.02)
|
||
{
|
||
imu_v += Chassis_IMU_data->MotionAccel_n[X] * integral_dt;
|
||
|
||
}
|
||
imu_x += imu_v * integral_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)
|
||
{
|
||
ChassisState = FAIL;
|
||
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 = (- 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;
|
||
|
||
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);
|
||
|
||
float separate_x[2] = { - x_r * DEGREE_2_RAD * RADIUS_WHEEL , x_l * DEGREE_2_RAD * RADIUS_WHEEL};
|
||
float separate_x_dot[2] = { - wheel_motor_r->measure.speed_rads * RADIUS_WHEEL , wheel_motor_l->measure.speed_rads * RADIUS_WHEEL};
|
||
// float separate_x[2] = { x,x};
|
||
// float separate_x_dot[2] = { x_dot,x_dot};
|
||
|
||
|
||
//Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
|
||
Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,separate_x,separate_x_dot,Chassis_IMU_data->MotionAccel_n);
|
||
if( ChassisState == FAIL )
|
||
{
|
||
if(abs(body_phi) <= 0.05 && abs(body_phi) <= 0.005 )
|
||
ChassisState = STAND;
|
||
}
|
||
if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE && last_chassis_mode != CHASSIS_ZERO_FORCE)
|
||
{
|
||
//倒地重新初始化卡尔曼滤波器一次
|
||
//estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
|
||
}
|
||
|
||
last_chassis_mode = chassis_cmd_recv.chassis_mode;
|
||
}
|
||
|
||
/* 机器人底盘控制核心任务 */
|
||
void ChassisTask()
|
||
{
|
||
// 后续增加没收到消息的处理(双板的情况)
|
||
// 获取新的控制信息
|
||
#ifdef ONE_BOARD
|
||
SubGetMessage(chassis_sub, &chassis_cmd_recv);
|
||
#endif
|
||
#ifdef CHASSIS_BOARD
|
||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||
#endif // CHASSIS_BOARD
|
||
|
||
//死亡情况下 关闭电机
|
||
if(referee_data->GameRobotState.power_management_chassis_output == 0)
|
||
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
|
||
|
||
//设置超电上限功率
|
||
float power_limit = referee_data->GameRobotState.chassis_power_limit;
|
||
power_limit = float_constrain(power_limit,40,120);
|
||
SuperCapSetPower(cap,power_limit);
|
||
|
||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||
HTMotorStop(motor_lf);
|
||
HTMotorStop(motor_rf);
|
||
HTMotorStop(motor_lb);
|
||
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);
|
||
//estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
|
||
}
|
||
else
|
||
{ // 正常工作
|
||
HTMotorEnable(motor_lf);
|
||
HTMotorEnable(motor_rf);
|
||
HTMotorEnable(motor_lb);
|
||
HTMotorEnable(motor_rb);
|
||
LKMotorEnable(wheel_motor_r);
|
||
LKMotorEnable(wheel_motor_l);
|
||
}
|
||
|
||
|
||
static int8_t init_flag = FALSE;
|
||
static uint8_t jump_flag = 0,last_jump_flag = 0;
|
||
static float jump_time = 0,shrink_time = 0;
|
||
// 根据控制模式设定旋转速度
|
||
switch (chassis_cmd_recv.chassis_mode)
|
||
{
|
||
case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
|
||
//chassis_cmd_recv.wz = 0;
|
||
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.08f * chassis_cmd_recv.offset_angle;
|
||
jump_flag = 0;
|
||
break;
|
||
case CHASSIS_LATERAL: //侧向对敌
|
||
chassis_cmd_recv.wz = 0.08f * loop_float_constrain(chassis_cmd_recv.offset_angle + 90,-180,180);
|
||
break;
|
||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||
{ //chassis_cmd_recv.wz = 1.0f;
|
||
if(last_jump_flag == 0)
|
||
{
|
||
jump_flag = 1;
|
||
jump_time = DWT_GetTimeline_ms();
|
||
}
|
||
else if(last_jump_flag == 1)
|
||
{
|
||
if (jump_time + 150 <= DWT_GetTimeline_ms()) {
|
||
jump_flag = 2;
|
||
shrink_time = DWT_GetTimeline_ms();
|
||
}
|
||
}
|
||
else if(last_jump_flag == 2)
|
||
{
|
||
if (shrink_time + 200 <= DWT_GetTimeline_ms()) {
|
||
jump_flag = 3;
|
||
}
|
||
}
|
||
last_jump_flag = jump_flag;
|
||
}
|
||
//chassis_cmd_recv.wz = 4000;
|
||
//chassis_cmd_recv.wz = 2.0f;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
|
||
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
|
||
static float sin_theta, cos_theta;
|
||
// cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||
// sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||
// sin_theta = 0; cos_theta = 1;
|
||
// chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
|
||
// chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
|
||
first_order_filter_cali(&vx_filter,chassis_cmd_recv.vx/5280 * 3);
|
||
chassis_vx = vx_filter.out;
|
||
|
||
|
||
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
|
||
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
|
||
// chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
|
||
// // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
|
||
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
|
||
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
|
||
|
||
//获取数据并计算状态量
|
||
Chassis_State_update();
|
||
if(chassis_cmd_recv.chassis_mode != CHASSIS_ZERO_FORCE) // 右侧开关状态[下],底盘跟随云台
|
||
{
|
||
target_x_set(&BalanceControl,chassis_vx);
|
||
target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
|
||
|
||
float turn_T = 0;
|
||
//离地情况下把转向环和roll轴补偿关掉
|
||
if(BalanceControl.fly_flag_r == FALSE || BalanceControl.fly_flag_l == FALSE)
|
||
{
|
||
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;
|
||
turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
|
||
}
|
||
else
|
||
{
|
||
turn_T = 0;
|
||
BalanceControl.leg_right.F_feedforward = 0;
|
||
BalanceControl.leg_left.F_feedforward = 0;
|
||
}
|
||
|
||
Balance_Control_Loop(&BalanceControl,jump_flag);
|
||
|
||
static float wz_feedback;
|
||
wz_feedback = Chassis_IMU_data->Gyro[Z];
|
||
|
||
static float wheel_current_r,wheel_current_l,Tr,Tl;
|
||
|
||
//轮电机滤波并输出
|
||
Tr = BalanceControl.LQR_r.control_vector[0];
|
||
Tl = BalanceControl.LQR_l.control_vector[0];
|
||
|
||
first_order_filter_cali(&wheel_r_filter,Tr+turn_T);
|
||
first_order_filter_cali(&wheel_l_filter,Tl-turn_T);
|
||
wheel_current_r = (wheel_r_filter.out)*LK_TORQUE_2_CMD;//(T+turn_T) * LK_TORQUE_2_CMD;
|
||
wheel_current_l = (wheel_l_filter.out)*LK_TORQUE_2_CMD;//(T-turn_T) * LK_TORQUE_2_CMD;
|
||
|
||
float_constrain(wheel_current_r,-4000,4000);
|
||
float_constrain(wheel_current_l,-4000,4000);
|
||
//关节电机滤波并输出
|
||
first_order_filter_cali(&ht_rf_filter,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
|
||
first_order_filter_cali(&ht_rb_filter,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
|
||
|
||
first_order_filter_cali(&ht_lf_filter,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
|
||
first_order_filter_cali(&ht_lb_filter,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
|
||
|
||
HTMotorSetRef(motor_rf,ht_rf_filter.out);
|
||
HTMotorSetRef(motor_rb,ht_rb_filter.out);
|
||
|
||
HTMotorSetRef(motor_lf,ht_lf_filter.out);
|
||
HTMotorSetRef(motor_lb,ht_lb_filter.out);
|
||
// 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,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);
|
||
}
|
||
else{
|
||
PIDClear(&BalanceControl.leg_length_PID);
|
||
PIDClear(&Turn_Loop_PID);
|
||
PIDClear(&Roll_Loop_PID);
|
||
PIDClear(&BalanceControl.leg_coordination_PID);
|
||
}
|
||
|
||
static uint8_t last_UI_refresh = 0 ;
|
||
if(chassis_cmd_recv.UI_refresh != last_UI_refresh)
|
||
MyUIInit();
|
||
last_UI_refresh = chassis_cmd_recv.UI_refresh;
|
||
//推送ui显示界面
|
||
ui_data.relative_angle = chassis_cmd_recv.offset_angle;
|
||
|
||
for (int i = 0; i < 6; ++i) {
|
||
ui_data.leg_pos_r[i] = BalanceControl.leg_right.legState.position[i];
|
||
ui_data.leg_pos_l[i] = BalanceControl.leg_left.legState.position[i];
|
||
}
|
||
|
||
ui_data.chassis_mode = chassis_cmd_recv.chassis_mode;
|
||
ui_data.shoot_mode = chassis_cmd_recv.shoot_mode;
|
||
ui_data.friction_mode = chassis_cmd_recv.friction_mode;
|
||
ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
|
||
ui_data.Chassis_Power_Data.cap_vol = (float)cap->cap_msg.cap_vol/100.0f;
|
||
ui_data.Chassis_Power_Data.input_vol = (float)cap->cap_msg.input_vol/100.0f;
|
||
|
||
|
||
|
||
// 推送反馈消息
|
||
chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
|
||
chassis_feedback_data.real_wz = Chassis_IMU_data->Gyro[Z];
|
||
chassis_feedback_data.power_management_chassis_output = referee_data->GameRobotState.power_management_chassis_output;
|
||
#ifdef ONE_BOARD
|
||
PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
|
||
#endif
|
||
#ifdef CHASSIS_BOARD
|
||
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
|
||
#endif // CHASSIS_BOARD
|
||
} |