zyx
/
engineering
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
engineering/application/gimbal/gimbal.cpp

563 lines
22 KiB
C++
Raw Permalink Blame History

#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"
#include "bsp_log.h"
#include "referee_VT.h"
#ifdef __cplusplus
}
#endif
#include "matrix.h"
#include "robotics.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DMMotorInstance *yaw_motor, *pitch_motor_l,*pitch_motor_r, *roll_motor;
static DMMotorInstance *diff_r_motor,*diff_l_motor;
//pitch轴双6006 双环pid算出力矩 一人一半
static PIDInstance pitch_spd_loop,pitch_angle_loop;
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 = 6.0;
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的控制信息
static gimbal_mode_e last_gimbal_mode; //上次模式 用于模式切换数据过渡
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;
first_order_filter_type_t Qset_filter[3]; //关节目标值滤波
float q_set[5] ; //五个关节的目标值
//机械臂参数初始化
//float arm_q[5] = {0}; // 机械臂各关节位置
robotics::Link link[5];
robotics::Serial_Link<5> engineer_arm(link);
Matrixf<4, 4> fk_T; //正运动学 末端变换矩阵
Matrixf<3, 1> fk_p; //正运动学 末端位置向量
Matrixf<3, 1> fk_rpy; //正运动学 末端欧拉角
Matrixf<5, 1> arm_q; //正运动学 末端关节位置
Matrixf<4, 4> T_cmd; //拟运动学 期望末端变换矩阵
Matrixf<5, 2> ik_q; //逆运动学 关节位置
Matrixf<5, 1> ik_q_cmd;//逆运动学 期望关节位置
Matrixf<3, 1> cmd_xyz; //逆运动学 末端期望位置
Matrixf<3, 3> cmd_R; //逆运动学 末端期望姿态
Matrixf<4, 1> cmd_quat; //逆运动学 末端期望姿态
Matrixf<3, 2> ik_q3; //逆运动学 球形手腕关节位置
Matrixf<3, 1> ik_q3_cmd;//逆运动学 球形手腕期望关节位置
const float l1 = 0.151 ,l3 = 0.350, l4 = 0;//0.139;
void Arm_Init()
{
link[0] = robotics::Link(0,l1,0,PI/2);
link[1] = robotics::Link(0,0,l3,PI/2);
link[2] = robotics::Link(0,0,0,-PI/2);
link[3] = robotics::Link(0,0,0,PI/2);
link[4] = robotics::Link(0,l4,0,0);
engineer_arm = robotics::Serial_Link<5>(link);
engineer_arm.ikine_analytic = robotics::my_analytic_ikine;
engineer_arm.ikine_analytic_check = robotics::check_ikine;
}
void GimbalInit()
{
Arm_Init();
Motor_Init_Config_s yaw_motor_config = {
.controller_param_init_config = {
.other_speed_feedback_ptr = &yaw_spd,
.speed_PID = {
.Kp = 0.0f,//3.5f,//5,
.Ki = 0.0f,
.Kd = 0,//0.02f,
.MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>(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,
};
Motor_Init_Config_s pitch_motor_config = {
.controller_param_init_config = {
.other_speed_feedback_ptr = &pitch_spd,
//.current_feedforward_ptr = &arm_gravity_feedforward,
},
.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 = DM6006,
};
PID_Init_Config_s pitch_spd_config= {
.Kp = 3.0f,
.Ki = 0.0f,
.Kd = 0.1f,
.MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 100,
};
PID_Init_Config_s pitch_angle_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(&pitch_spd_loop,&pitch_spd_config);
PIDInit(&pitch_angle_loop,&pitch_angle_config);
Motor_Init_Config_s roll_motor_config = {
.controller_param_init_config = {
.other_speed_feedback_ptr = &roll_spd,
.speed_PID = {
.Kp = 0.4f,
.Ki = 0.0f,
.Kd = 0.01f,//0.02f,
.MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 10.0F,
},
.angle_PID = {
.Kp = 15.0f,//25.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,
};
Motor_Init_Config_s diff_motor_config = {
.controller_param_init_config = {
.speed_PID = {
.Kp = 0.6f,
.Ki = 0.1f,
.Kd = 0.02f,
.MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 10.0F,
.Output_LPF_RC = 0.02F,
},
},
.controller_setting_init_config = {
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP,
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = OTHER_FEED,
//.feedforward_flag = CURRENT_FEEDFORWARD,
},
.motor_type = DM4310,
};
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.05f;
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);
const float Qset_filter_num = 0.1f;
first_order_filter_init(&Qset_filter[0],5e-3,&Qset_filter_num);
first_order_filter_init(&Qset_filter[1],5e-3,&Qset_filter_num);
first_order_filter_init(&Qset_filter[2],5e-3,&Qset_filter_num);
PID_Init_Config_s diff_pitch_config= {
.Kp = 30.0f,//20.0F,//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_roll_config= {
.Kp = 50.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);
//需要按id顺序初始化 否则有电机不能正确运行
//决定放弃yaw轴
// 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);
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_l = DMMotorInit(&pitch_motor_config);
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);
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);
pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！
pitch_motor_config.can_init_config.rx_id = 12;
pitch_motor_r = DMMotorInit(&pitch_motor_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));
// //上电校准两个4310零点
// DMMotorCaliEncoder(diff_r_motor);
// DMMotorCaliEncoder(diff_l_motor);
////水平方向3.02 矿仓 0
// DMMotorCaliEncoder(pitch_motor_l);
// DMMotorCaliEncoder(roll_motor);
q_set[1] = PITCH_MIN * DEGREE_2_RAD; //上电给pitch轴赋值
}
//#define PITCH_OFFSET 3.02F //pitch轴水平时编码器值
//static void DMMotroEnable()
//{
// if(gimbal_cmd_recv.MotorEnableFlag)
// {
// DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor);
// DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor);
// DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor);
// DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor );
// DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor );
// }
//}
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
float debug_diff_kp = 0.2f,debug_diff_ki=0,debug_diff_kd = 0.01f;
float diff_motor_r_offset = 0; float diff_motor_l_offset = 0;
void GimbalTask()
{
diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
// 获取云台控制数据
// 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
/* 控制参数计算 ------------------------------------------------------------------------*/
//大臂重力补偿力矩
arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position);
//电机速度滤波
first_order_filter_cali(&pitch_spd_filter,pitch_motor_l->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_r_angle = diff_r_motor->measure.total_angle - diff_motor_r_offset;
float diff_l_angle = diff_l_motor->measure.total_angle - diff_motor_l_offset;
float diff_pitch_angle = (diff_r_angle + (-diff_l_angle))/2;
float diff_roll_angle = (diff_r_angle - (-diff_l_angle))/2 * 18/52;
/* 控制参数计算 ------------------------------------------------------------------------*/
//正运动学
arm_q[0][0] = 0;//yaw_motor->measure.position;
arm_q[1][0] = -(pitch_motor_l->measure.position);
arm_q[2][0] = roll_motor->measure.position;
arm_q[3][0] = diff_pitch_angle;
arm_q[4][0] = diff_roll_angle;
fk_T = engineer_arm.fkine(arm_q);
fk_p = robotics::t2p(fk_T);
fk_rpy = robotics::t2rpy(fk_T);
//逆运动学
// ik_q = engineer_arm.ikine_analytic(fk_T);
//
// ik_q_cmd = ik_q.block<5,1>(0,0);
// ik_q3 = robotics::spherical_wrist_ikine(robotics::t2r(fk_T),arm_q[0][0],arm_q[1][0]);
static uint8_t last_zero_flag = 0;
if(gimbal_cmd_recv.set_zero_flag != last_zero_flag)
{
diff_motor_r_offset = diff_r_motor->measure.total_angle;
diff_motor_l_offset = diff_l_motor->measure.total_angle;
PIDClear(&diff_pitch_loop);
PIDClear(&diff_roll_loop);
PIDClear(&diff_r_motor->motor_controller.speed_PID);
PIDClear(&diff_l_motor->motor_controller.speed_PID);
//目标值也清零
q_set[3] = 0 ;
q_set[4] = 0 ;
}
last_zero_flag = gimbal_cmd_recv.set_zero_flag;
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
{
DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r);
DMMotorStop(yaw_motor);DMMotorStop(roll_motor);
DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor);
//不使能模式下 目标值为当前值 防止使能甩大臂
q_set[1] = pitch_motor_l->measure.position;
}
else
{
DMMotorEnable(pitch_motor_l);DMMotorEnable(pitch_motor_r);
DMMotorEnable(yaw_motor);DMMotorEnable(roll_motor);
DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor);
}
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_MANUAL_MODE) //各关节分开控制
{
q_set[0] = 0;
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
q_set[2] += gimbal_cmd_recv.roll * DEGREE_2_RAD;
q_set[3] += gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD;
q_set[4] += gimbal_cmd_recv.diff_roll * DEGREE_2_RAD;
}
//地矿和银矿模式直接计算角度 不逆解了
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_FLOOR_MODE)
{
q_set[0] = 0;
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
q_set[2] = 0;
q_set[3] = -arm_q[1][0]-PI/2;
q_set[4] = 0;
}
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_SILVER_MODE)
{
q_set[0] = 0;
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
q_set[2] = 0;
q_set[3] = -arm_q[1][0];
q_set[4] = 0;
}
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_STORAGE_MODE)
{
q_set[0] = 0;
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
q_set[2] = 0;
q_set[3] = -arm_q[1][0] + 7*PI/6;
q_set[4] = 0;
}
//if(gimbal_cmd_recv.gimbal_mode != GIMBAL_MANUAL_MODE && gimbal_cmd_recv.gimbal_mode != GIMBAL_ZERO_FORCE)
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
{
if(last_gimbal_mode == GIMBAL_MANUAL_MODE)
{
//切换至逆解模式时，目标值设置为当前值
cmd_R = robotics::t2r( fk_T);
}
for (int i = 0; i < 4; ++i) {
cmd_quat[i][0] = gimbal_cmd_recv.quat[i];
}
// if(gimbal_cmd_recv.gimbal_mode == GIMBAL_SILVER_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_FLOOR_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_STORAGE_MODE)
// {
// float trans_rpy_data[3] = {gimbal_cmd_recv.rpy[0]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[1]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[2]*DEGREE_2_RAD};
// Matrixf<3,1> trans_rpy(trans_rpy_data);
// cmd_R = robotics::rpy2r(trans_rpy);
// }
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
{
float trans_data[3] = {0,-PI,0};
Matrixf<3,1> trans(trans_data);
cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans); //转到基座坐标系下的姿态
}
ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]);
//后三关节的误差 选用误差小的一组解
float err[2][3] = {0};
Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
for (int i = 0; i < 2; ++i) {
Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
for (int j = 0; j < 3; ++j) {
err[i][j] = loop_float_constrain(ik[j][0] - arm_q3[j][0],-PI,PI);
}
}
float err1 = abs(err[1][0]) + abs(err[1][1]) + abs(err[1][2]);
float err0 = abs(err[0][0]) + abs(err[0][1]) + abs(err[0][2]);
//选用 总 误差小的一组解
// if (err1 >= err0)
// ik_q3_cmd = ik_q3.block<3,1>(0,0);
// else
// ik_q3_cmd = ik_q3.block<3,1>(0,1);
// // 选用 roll 误差小的一组解
float roll_err1 = ik_q3[0][0] - arm_q3[0][0];
roll_err1 = loop_float_constrain(roll_err1,-PI,PI);
float roll_err2 = ik_q3[1][0] - arm_q3[0][0];
roll_err2 = loop_float_constrain(roll_err2,-PI,PI);
if (abs(roll_err1) <= abs(roll_err2))
ik_q3_cmd = ik_q3.block<3,1>(0,0);
else
ik_q3_cmd = ik_q3.block<3,1>(0,1);
//选用 第一组解
//ik_q3_cmd = ik_q3.block<3,1>(0,0);
float q_err[3] = {ik_q3_cmd[0][0] - arm_q3[0][0],
ik_q3_cmd[1][0] - arm_q3[1][0],
ik_q3_cmd[2][0] - arm_q3[2][0]};
ik_q3_cmd[0][0] += loop_float_constrain(ik_q3_cmd[0][0] - arm_q3[0][0],-PI,PI);
ik_q3_cmd[1][0] += loop_float_constrain(ik_q3_cmd[1][0] - arm_q3[1][0],-PI,PI);
ik_q3_cmd[2][0] += loop_float_constrain(ik_q3_cmd[2][0] - arm_q3[2][0],-PI,PI);
// ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
//ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
// ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
for (int i = 0; i < 3; ++i) {
first_order_filter_cali(&Qset_filter[i],ik_q3_cmd[i][0]);
q_set[i+2] = Qset_filter[i].out;
}
//大臂pitch用遥控器数据
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
}
//保存上次模式
last_gimbal_mode = gimbal_cmd_recv.gimbal_mode;
q_set[1] = float_constrain(q_set[1],PITCH_MIN* DEGREE_2_RAD,PITCH_MAX* DEGREE_2_RAD);
q_set[2] = float_constrain(q_set[2],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
q_set[3] = float_constrain(q_set[3],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
//roll轴不做限位处理
//q_set[4] = float_constrain(q_set[4],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[3]);
float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[4]);
float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;
//pitch轴双环PID pitch yaw先遥控器控制
float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,q_set[1]);
float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;
DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
DMMotorSetRef(yaw_motor,q_set[0]);
DMMotorSetRef(roll_motor,q_set[2]);
DMMotorSetRef(diff_r_motor,r_speed_set);
DMMotorSetRef(diff_l_motor,-l_speed_set);
// 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ...
// 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.diff_pitch_feedback = diff_pitch_angle * RAD_2_DEGREE;
gimbal_feedback_data.pitch_feedback = - pitch_motor_l->measure.position * RAD_2_DEGREE;
// 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}
Reference in New Issue View Git Blame Copy Permalink