314 lines
13 KiB
C++
314 lines
13 KiB
C++
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
#include "gimbal.h"
|
||
#include "robot_def.h"
|
||
#include "dji_motor.h"
|
||
#include "dmmotor.h"
|
||
#include "ins_task.h"
|
||
#include "message_center.h"
|
||
#include "general_def.h"
|
||
#include "bmi088.h"
|
||
#include "user_lib.h"
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
#include "matrix.h"
|
||
#include "robotics.h"
|
||
|
||
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
||
static DMMotorInstance *yaw_motor, *pitch_motor, *roll_motor;
|
||
static DMMotorInstance *diff_r_motor,*diff_l_motor;
|
||
|
||
static PIDInstance diff_pitch_loop,diff_roll_loop;
|
||
static PIDInstance diff_pitch_spd_loop,diff_roll_spd_loop;
|
||
|
||
float arm_gravity_feedforward = 0;
|
||
float GRAVITY_COMP = 5.5;
|
||
|
||
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
|
||
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
|
||
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
|
||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
|
||
|
||
first_order_filter_type_t pitch_spd_filter,yaw_spd_filter,roll_spd_filter;
|
||
static float pitch_spd,yaw_spd,roll_spd;
|
||
|
||
first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter;
|
||
static float diff_r_spd,diff_l_spd;
|
||
|
||
void GimbalInit()
|
||
{
|
||
Motor_Init_Config_s yaw_motor_config = {
|
||
.controller_param_init_config = {
|
||
.other_speed_feedback_ptr = &yaw_spd,
|
||
.speed_PID = {
|
||
.Kp = 3,//5,
|
||
.Ki = 3.0f,
|
||
.Kd = 0.02f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 10.0F,
|
||
},
|
||
.angle_PID = {
|
||
.Kp = 10.0f,//13.0f,
|
||
.Ki = 0.0f,
|
||
.Kd = 0.0f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 10.0F,
|
||
},
|
||
},
|
||
.controller_setting_init_config = {
|
||
.outer_loop_type = ANGLE_LOOP,
|
||
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
||
.angle_feedback_source = MOTOR_FEED,
|
||
.speed_feedback_source = OTHER_FEED,
|
||
.feedforward_flag = CURRENT_FEEDFORWARD,
|
||
},
|
||
.motor_type = DM6006,
|
||
};
|
||
|
||
yaw_motor_config.can_init_config.can_handle = &hcan2;
|
||
yaw_motor_config.can_init_config.tx_id = 1;
|
||
yaw_motor_config.can_init_config.rx_id = 2;
|
||
yaw_motor = DMMotorInit(&yaw_motor_config);
|
||
|
||
Motor_Init_Config_s pitch_motor_config = {
|
||
.controller_param_init_config = {
|
||
.other_speed_feedback_ptr = &pitch_spd,
|
||
.current_feedforward_ptr = &arm_gravity_feedforward,
|
||
.speed_PID = {
|
||
.Kp = 2.5f,
|
||
.Ki = 0.8f,
|
||
.Kd = 0.0f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 10.0F,
|
||
},
|
||
.angle_PID = {
|
||
.Kp = 8.0f,
|
||
.Ki = 0.0f,
|
||
.Kd = 0.0f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 10.0F,
|
||
},
|
||
},
|
||
.controller_setting_init_config = {
|
||
.outer_loop_type = ANGLE_LOOP,
|
||
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
||
.angle_feedback_source = MOTOR_FEED,
|
||
.speed_feedback_source = OTHER_FEED,
|
||
.feedforward_flag = CURRENT_FEEDFORWARD,
|
||
},
|
||
.motor_type = DM6006,
|
||
};
|
||
|
||
pitch_motor_config.can_init_config.can_handle = &hcan2;
|
||
pitch_motor_config.can_init_config.tx_id = 3;
|
||
pitch_motor_config.can_init_config.rx_id = 4;
|
||
pitch_motor = DMMotorInit(&pitch_motor_config);
|
||
|
||
//@todo：roll轴机械固定不牢 待细调
|
||
Motor_Init_Config_s roll_motor_config = {
|
||
.controller_param_init_config = {
|
||
.other_speed_feedback_ptr = &roll_spd,
|
||
.speed_PID = {
|
||
.Kp = 0.8f,
|
||
.Ki = 0.3f,
|
||
.Kd = 0.02f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 10.0F,
|
||
},
|
||
.angle_PID = {
|
||
.Kp = 10.0f,
|
||
.Ki = 0.0f,
|
||
.Kd = 0.0f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 10.0F,
|
||
},
|
||
},
|
||
.controller_setting_init_config = {
|
||
.outer_loop_type = ANGLE_LOOP,
|
||
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
||
.angle_feedback_source = MOTOR_FEED,
|
||
.speed_feedback_source = OTHER_FEED,
|
||
//.feedforward_flag = CURRENT_FEEDFORWARD,
|
||
},
|
||
.motor_type = DM4310,
|
||
};
|
||
|
||
roll_motor_config.can_init_config.can_handle = &hcan2;
|
||
roll_motor_config.can_init_config.tx_id = 5;
|
||
roll_motor_config.can_init_config.rx_id = 6;
|
||
roll_motor = DMMotorInit(&roll_motor_config);
|
||
|
||
Motor_Init_Config_s diff_motor_config = {
|
||
.controller_setting_init_config = {
|
||
.outer_loop_type = OPEN_LOOP,
|
||
.close_loop_type = OPEN_LOOP,
|
||
.angle_feedback_source = MOTOR_FEED,
|
||
.speed_feedback_source = OTHER_FEED,
|
||
//.feedforward_flag = CURRENT_FEEDFORWARD,
|
||
},
|
||
.motor_type = DM4310,
|
||
};
|
||
|
||
diff_motor_config.can_init_config.can_handle = &hcan2;
|
||
diff_motor_config.can_init_config.tx_id = 7;
|
||
diff_motor_config.can_init_config.rx_id = 8;
|
||
diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
|
||
diff_r_motor = DMMotorInit(&diff_motor_config);
|
||
|
||
diff_motor_config.can_init_config.can_handle = &hcan2;
|
||
diff_motor_config.can_init_config.tx_id = 9;
|
||
diff_motor_config.can_init_config.rx_id = 10;
|
||
diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
|
||
diff_l_motor = DMMotorInit(&diff_motor_config);
|
||
|
||
const float spd_filter_num = 0.05f;
|
||
first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num);
|
||
const float spd_filter_num_yaw = 0.1f;
|
||
first_order_filter_init(&yaw_spd_filter,5e-3,&spd_filter_num_yaw);
|
||
const float spd_filter_num_roll = 0.05f;
|
||
first_order_filter_init(&roll_spd_filter,5e-3,&spd_filter_num_roll);
|
||
|
||
const float spd_filter_num_diff = 0.05f;
|
||
first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff);
|
||
first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff);
|
||
|
||
PID_Init_Config_s diff_pitch_config= {
|
||
.Kp = 15.0f,
|
||
.Ki = 0,
|
||
.Kd = 0,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 100,
|
||
};
|
||
PIDInit(&diff_pitch_loop,&diff_pitch_config);
|
||
|
||
PID_Init_Config_s diff_pitch_spd_config= {
|
||
.Kp = 0.6f,
|
||
.Ki = 2.0f,
|
||
.Kd = 0.01f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 100,
|
||
};
|
||
PIDInit(&diff_pitch_spd_loop,&diff_pitch_spd_config);
|
||
|
||
PID_Init_Config_s diff_roll_config= {
|
||
.Kp = 14.0f,
|
||
.Ki = 0,
|
||
.Kd = 0,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 100,
|
||
};
|
||
PIDInit(&diff_roll_loop,&diff_roll_config);
|
||
|
||
PID_Init_Config_s diff_roll_spd_config= {
|
||
.Kp = 1.0f,
|
||
.Ki = 1.0f,
|
||
.Kd = 0.02f,
|
||
.MaxOut = 10,
|
||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||
.IntegralLimit = 100,
|
||
};
|
||
PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);
|
||
|
||
|
||
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
|
||
// YAW
|
||
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||
}
|
||
|
||
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
|
||
void GimbalTask()
|
||
{
|
||
/* Example 1: PUMA560 ------------------------------------------------------------------------*/
|
||
float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0};
|
||
float data[18]={0, -8.5e-2, 0, 0, 0, 0,
|
||
13.225e-2, 0, 0, 0, 0, 0,
|
||
0, 3.7e-2, 8.525e-2, 0, 0, 0};
|
||
Matrixf<3, 6> rc(data);
|
||
Matrixf<3, 3> I[6];
|
||
// 获取云台控制数据
|
||
// 后续增加未收到数据的处理
|
||
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||
//大臂重力补偿力矩
|
||
arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position);
|
||
//电机速度滤波
|
||
first_order_filter_cali(&pitch_spd_filter,pitch_motor->measure.velocity);
|
||
pitch_spd = pitch_spd_filter.out;
|
||
first_order_filter_cali(&yaw_spd_filter,yaw_motor->measure.velocity);
|
||
yaw_spd = yaw_spd_filter.out;
|
||
first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity);
|
||
roll_spd = roll_spd_filter.out;
|
||
|
||
//手腕关节
|
||
first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity);
|
||
diff_r_spd = diff_r_spd_filter.out;
|
||
first_order_filter_cali(&diff_l_spd_filter,diff_l_motor->measure.velocity);
|
||
diff_l_spd = diff_l_spd_filter.out;
|
||
|
||
float diff_pitch_angle = (diff_r_motor->measure.position + (-diff_l_motor->measure.position))/2;
|
||
float diff_roll_angle = (diff_r_motor->measure.position - (-diff_l_motor->measure.position))/2 * 18/52;
|
||
|
||
float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD);
|
||
float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD);
|
||
|
||
float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2;
|
||
float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52;
|
||
float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out);
|
||
float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out);
|
||
|
||
float r_speed_set = pitch_out + roll_out;
|
||
float l_speed_set = pitch_out - roll_out;
|
||
|
||
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
|
||
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
|
||
switch (gimbal_cmd_recv.gimbal_mode)
|
||
{
|
||
// 停止
|
||
case GIMBAL_ZERO_FORCE:
|
||
DMMotorStop(yaw_motor);
|
||
DMMotorStop(pitch_motor);
|
||
DMMotorStop(roll_motor);
|
||
DMMotorStop(diff_r_motor);
|
||
DMMotorStop(diff_l_motor);
|
||
|
||
break;
|
||
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
|
||
case GIMBAL_GYRO_MODE: // 后续只保留此模式
|
||
DMMotorEnable(yaw_motor);
|
||
DMMotorEnable(pitch_motor);
|
||
DMMotorEnable(roll_motor);
|
||
DMMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
|
||
DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
|
||
DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD);
|
||
|
||
DMMotorEnable(diff_r_motor);
|
||
DMMotorEnable(diff_l_motor);
|
||
DMMotorSetRef(diff_r_motor,r_speed_set);
|
||
DMMotorSetRef(diff_l_motor,-l_speed_set);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
// 在合适的地方添加pitch重力补偿前馈力矩
|
||
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
|
||
// ...
|
||
|
||
// 设置反馈数据,主要是imu和yaw的ecd
|
||
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
|
||
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
|
||
|
||
// 推送消息
|
||
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
||
} |