mirror of
https://gitee.com/dlmu-cone/half-steering-basic-framework
synced 2026-10-23 17:42:20 +08:00
first commit
This commit is contained in:
TuxMonkey
158
application/gimbal/gimbal.c
Normal file
158
application/gimbal/gimbal.c
Normal file
@@ -0,0 +1,158 @@
|
||||
#include "gimbal.h"
|
||||
#include "robot_def.h"
|
||||
#include "dji_motor.h"
|
||||
#include "ins_task.h"
|
||||
#include "message_center.h"
|
||||
#include "general_def.h"
|
||||
|
||||
#include "bmi088.h"
|
||||
|
||||
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
||||
static DJIMotorInstance *yaw_motor, *pitch_motor;
|
||||
|
||||
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的控制信息
|
||||
|
||||
void GimbalInit()
|
||||
{
|
||||
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
|
||||
// YAW
|
||||
Motor_Init_Config_s yaw_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 5,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 0.685, // 8
|
||||
.Ki = 0.315,
|
||||
.Kd = 0.022,
|
||||
//.DeadBand = 0.1,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
|
||||
.IntegralLimit = 1060,
|
||||
.CoefA = 20,
|
||||
.CoefB = 0.2,
|
||||
.MaxOut = 4000,
|
||||
.Derivative_LPF_RC = 0.001
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 8000, // 50
|
||||
.Ki = 5000, // 200
|
||||
.Kd = 0,
|
||||
.CoefB = 1,
|
||||
.CoefA = 10,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
|
||||
.IntegralLimit = 7000,
|
||||
.MaxOut = 25000,
|
||||
},
|
||||
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
|
||||
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
||||
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020};
|
||||
// PITCH
|
||||
Motor_Init_Config_s pitch_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 4,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 10, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 100,
|
||||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 50, // 50
|
||||
.Ki = 350, // 350
|
||||
.Kd = 0, // 0
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 2500,
|
||||
.MaxOut = 20000,
|
||||
},
|
||||
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
|
||||
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
||||
.other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020,
|
||||
};
|
||||
// 电机对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));
|
||||
}
|
||||
|
||||
/* 机器人云台控制核心任务,后续考虑只保留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)
|
||||
{
|
||||
// 停止
|
||||
case GIMBAL_ZERO_FORCE:
|
||||
DJIMotorStop(yaw_motor);
|
||||
DJIMotorStop(pitch_motor);
|
||||
break;
|
||||
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
|
||||
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);
|
||||
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
|
||||
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
|
||||
break;
|
||||
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
|
||||
case GIMBAL_FREE_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);
|
||||
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
|
||||
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
|
||||
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;
|
||||
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.total_angle;
|
||||
// 推送消息
|
||||
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
||||
}
|
||||
16
application/gimbal/gimbal.h
Normal file
16
application/gimbal/gimbal.h
Normal file
@@ -0,0 +1,16 @@
|
||||
#ifndef GIMBAL_H
|
||||
#define GIMBAL_H
|
||||
|
||||
/**
|
||||
* @brief 初始化云台,会被RobotInit()调用
|
||||
*
|
||||
*/
|
||||
void GimbalInit();
|
||||
|
||||
/**
|
||||
* @brief 云台任务
|
||||
*
|
||||
*/
|
||||
void GimbalTask();
|
||||
|
||||
#endif // GIMBAL_H
|
||||
13
application/gimbal/gimbal.md
Normal file
13
application/gimbal/gimbal.md
Normal file
@@ -0,0 +1,13 @@
|
||||
# gimbal
|
||||
|
||||
|
||||
|
||||
## 工作流程
|
||||
|
||||
初始化pitch和yaw电机以及一个imu。订阅gimbal_cmd消息（来自robot_cmd）并发布gimbal_feed话题。
|
||||
|
||||
1. 从消息中心获取gimbal_cmd话题的消息
|
||||
2. 根据消息中的控制模式进行模式切换，如果急停则关闭所有电机
|
||||
3. 由设定的模式，进行电机反馈数据来源的切换，修改反馈数据指针，设置前馈控制数据指针等。
|
||||
4. 设置反馈数据，包括yaw电机的绝对角度和imu数据
|
||||
5. 推送反馈数据到gimbal_feed话题下
|
||||
Reference in New Issue
Block a user