sentry_gimbal_hzz/application/gimbal/gimbal.c

193 lines
7.5 KiB
C
Raw Normal View History

2023-03-23 18:57:54 +08:00
//
#include "gimbal.h"
2022-12-02 22:17:10 +08:00
#include "robot_def.h"
2023-03-23 18:57:54 +08:00
//
2022-12-02 22:17:10 +08:00
#include "dji_motor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
2023-01-02 23:20:35 +08:00
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor; // yaw电机
static DJIMotorInstance *pitch_motor; // pitch电机
2022-12-03 21:39:31 +08:00
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的控制信息
BMI088Instance* imu;
void GimbalInit()
{
BMI088_Init_Config_s imu_config = {
.spi_acc_config={
.GPIOx=CS1_ACCEL_GPIO_Port,
.cs_pin=CS1_ACCEL_Pin,
.spi_handle=&hspi1,
},
.spi_gyro_config={
.GPIOx=CS1_GYRO_GPIO_Port,
.cs_pin=CS1_GYRO_Pin,
.spi_handle=&hspi1,
},
.acc_int_config={
.exti_mode=EXTI_TRIGGER_FALLING,
.GPIO_Pin=INT_ACC_Pin,
.GPIOx=INT_ACC_GPIO_Port,
},
.gyro_int_config={
.exti_mode=EXTI_TRIGGER_FALLING,
.GPIO_Pin=INT_GYRO_Pin,
.GPIOx=INT_GYRO_GPIO_Port,
},
.heat_pid_config={
.Kp=0.0f,
.Kd=0.0f,
.Ki=0.0f,
.MaxOut=0.0f,
.DeadBand=0.0f,
},
.heat_pwm_config={
.channel=TIM_CHANNEL_1,
.htim=&htim1,
},
.cali_mode=BMI088_CALIBRATE_ONLINE_MODE,
.work_mode=BMI088_BLOCK_PERIODIC_MODE,
};
2023-03-23 18:57:54 +08:00
// imu=BMI088Register(&imu_config);
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
Motor_Init_Config_s yaw_config = {
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 1,
},
.controller_param_init_config = {
.angle_PID = {
2023-03-23 18:57:54 +08:00
.Kp = 8, //8
.Ki = 0,
.Kd = 0,
2023-03-23 18:57:54 +08:00
.DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
2023-03-23 18:57:54 +08:00
.Kp = 50,//40
.Ki = 200,
.Kd = 0,
2023-03-23 18:57:54 +08:00
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
},
2023-01-02 23:20:35 +08:00
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
2023-03-23 18:57:54 +08:00
.other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2],
},
.controller_setting_init_config = {
2023-03-23 18:57:54 +08:00
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
2023-03-23 18:57:54 +08:00
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020};
// PITCH
Motor_Init_Config_s pitch_config = {
.can_init_config = {
2023-03-23 18:57:54 +08:00
.can_handle = &hcan2,
.tx_id = 2,
},
.controller_param_init_config = {
.angle_PID = {
2023-03-23 18:57:54 +08:00
.Kp =10,//10
.Ki = 0,
.Kd = 0,
2023-03-23 18:57:54 +08:00
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.IntegralLimit =100,
.MaxOut = 500,
},
.speed_PID = {
2023-03-23 18:57:54 +08:00
.Kp=50,//50
.Ki =350,//350
.Kd =0,//0.1
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.IntegralLimit =2500,
.MaxOut = 20000,
},
2023-01-02 23:20:35 +08:00
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
2023-03-23 18:57:54 +08:00
.other_speed_feedback_ptr=(&gimba_IMU_data->Gyro[0]),
},
.controller_setting_init_config = {
2023-03-23 18:57:54 +08:00
.angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
2023-03-23 18:57:54 +08:00
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020,
};
2022-12-05 15:03:45 +08:00
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
pitch_motor = DJIMotorInit(&pitch_config);
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}
2023-01-01 17:32:22 +08:00
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
// 获取云台控制数据
// 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
switch (gimbal_cmd_recv.gimbal_mode)
{
2022-12-05 15:03:45 +08:00
// 停止
case GIMBAL_ZERO_FORCE:
DJIMotorStop(yaw_motor);
DJIMotorStop(pitch_motor);
break;
2022-12-05 15:03:45 +08:00
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
2023-01-01 17:32:22 +08:00
case GIMBAL_GYRO_MODE: // 后续只保留此模式
DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
2022-12-05 15:03:45 +08:00
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break;
2022-12-05 15:03:45 +08:00
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
2023-01-01 17:32:22 +08:00
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor);
2023-03-23 18:57:54 +08:00
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
2022-12-05 15:03:45 +08:00
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break;
default:
break;
}
// 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ...
2023-01-01 17:32:22 +08:00
// 设置反馈数据,主要是imu和yaw的ecd
2023-01-02 23:20:35 +08:00
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
// 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}