280 lines
10 KiB
C
280 lines
10 KiB
C
#include "gimbal.h"
|
|
#include "robot_def.h"
|
|
#include "dji_motor.h"
|
|
#include "ECmotor/ECA8210.h"
|
|
#include "ins_task.h"
|
|
#include "message_center.h"
|
|
#include "general_def.h"
|
|
|
|
#include "bmi088.h"
|
|
#include "vofa.h"
|
|
|
|
#include "power_meter.h"
|
|
#include "referee_task.h"
|
|
|
|
|
|
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
|
static DJIMotorInstance *yaw_motor, *pitch_motor;
|
|
|
|
static DJIMotorInstance *big_yaw_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的控制信息
|
|
|
|
|
|
static float current_feedforward = 0.0f;//3000.0f;
|
|
static float GRAVITY_FEED = 0; //重力补偿前馈力矩系数
|
|
sin_input_generate_t sinInputGenerate;
|
|
void GimbalInit()
|
|
{
|
|
|
|
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 = {
|
|
.Kp = 0.5, // 8
|
|
.Ki = 0,
|
|
.Kd = 0,
|
|
.DeadBand = 0.1,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 100,
|
|
|
|
.MaxOut = 500,
|
|
},
|
|
.speed_PID = {
|
|
.Kp = 1e4, // 50
|
|
.Ki = 5000, // 200
|
|
.Kd = 0,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 10000,
|
|
.MaxOut = 20000,
|
|
},
|
|
.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 = SPEED_LOOP | ANGLE_LOOP,
|
|
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
|
},
|
|
.motor_type = GM6020,
|
|
.motor_control_type = CURRENT_CONTROL
|
|
};
|
|
// PITCH
|
|
Motor_Init_Config_s pitch_config = {
|
|
.can_init_config = {
|
|
.can_handle = &hcan2,
|
|
.tx_id = 1,
|
|
},
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 0.8f, // 10
|
|
.Ki = 0,
|
|
.Kd = 0.0f,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 100,
|
|
.MaxOut = 500,
|
|
},
|
|
.speed_PID = {
|
|
.Kp = 5000.0f,//2500, // 50
|
|
.Ki = 2000.0f,//200, // 350
|
|
.Kd = 0, // 0
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 10000,
|
|
.MaxOut = 30000,
|
|
},
|
|
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
|
|
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
|
.other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
|
|
.current_feedforward_ptr = ¤t_feedforward,
|
|
|
|
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP,
|
|
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
|
.feedforward_flag = CURRENT_FEEDFORWARD,
|
|
},
|
|
.motor_type = GM6020,
|
|
.motor_control_type = CURRENT_CONTROL,//CURRENT_CONTROL
|
|
};
|
|
|
|
|
|
// 电机对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));
|
|
|
|
//sin_input_frequency_init(&sinInputGenerate);
|
|
}
|
|
|
|
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
|
|
void GimbalTask()
|
|
{
|
|
// 获取云台控制数据
|
|
// 后续增加未收到数据的处理
|
|
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
|
|
|
// if(gimbal_cmd_recv.pitch_motor_ecd + (pitch_add) >= PITCH_MAX_ECD)
|
|
// {
|
|
// if(pitch_add > 0) gimbal_cmd_send.pitch = PITCH_MAX_ECD;
|
|
// // pitch_add = PITCH_MAX_ECD - gimbal_fetch_data.pitch_motor_ecd;
|
|
// }
|
|
// else if(gimbal_cmd_recv.pitch_motor_ecd + (pitch_add) <= PITCH_MIN_ECD)
|
|
// {
|
|
// if(pitch_add < 0) gimbal_cmd_send.pitch = PITCH_MIN_ECD;
|
|
// // pitch_add = PITCH_MIN_ECD - gimbal_fetch_data.pitch_motor_ecd;
|
|
// }
|
|
|
|
// @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电机角度反馈计算出当前配重下的重力矩
|
|
// ...
|
|
|
|
float input = sin_input_generate(&sinInputGenerate);
|
|
|
|
//ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0);
|
|
|
|
float theta = gimba_IMU_data->Pitch * DEGREE_2_RAD;
|
|
|
|
current_feedforward = GRAVITY_FEED * arm_cos_f32(theta);
|
|
|
|
float vofa_send_data[6];
|
|
vofa_send_data[0] = pitch_motor->measure.speed_aps;
|
|
vofa_send_data[1] = gimba_IMU_data->Pitch;
|
|
vofa_send_data[2] = gimba_IMU_data->Gyro[0];
|
|
vofa_send_data[3] = pitch_motor->motor_controller.speed_PID.Ref;
|
|
vofa_send_data[4] = pitch_motor->motor_controller.speed_PID.Measure;
|
|
vofa_send_data[5] = pitch_motor->measure.angle_single_round;
|
|
|
|
vofa_justfloat_output(vofa_send_data,24,&huart1);
|
|
|
|
|
|
// 设置反馈数据,主要是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.pitch_motor_ecd = pitch_motor->measure.angle_single_round + pitch_motor->motor_controller.angle_PID.Err;
|
|
|
|
|
|
// 推送消息
|
|
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
|
}
|
|
|
|
|
|
//void sin_input_generate(float frequency, int count)
|
|
//{
|
|
// static uint32_t cnt;
|
|
// static float time;
|
|
// while(time>=count*(1/frequency))
|
|
// {
|
|
// float deltaT=DWT_GetDeltaT(&cnt);
|
|
// time += deltaT;
|
|
//
|
|
// float input = arm_sin_f32(2*PI*frequency*time);
|
|
// DJIMotorSetRef(yaw_motor,input);
|
|
// }
|
|
//}
|
|
|
|
void sin_input_frequency_init(sin_input_generate_t* InputGenerate)
|
|
{
|
|
for(int i=0;i<43;i++)
|
|
{
|
|
InputGenerate->frequency[i] = 1.0 + 0.5*i;
|
|
}
|
|
for(int i=0;i<9;i++)
|
|
{
|
|
InputGenerate->frequency[i+43] = 24.0 + 2.0*i;
|
|
}
|
|
for(int i=0;i<8;i++)
|
|
{
|
|
InputGenerate->frequency[i+43+9] = 50 + 10*i;
|
|
}
|
|
InputGenerate->frequency[60] = 200;
|
|
InputGenerate->frequency[61] = 250;
|
|
InputGenerate->frequency[62] = 333;
|
|
InputGenerate->frequency[63] = 500;
|
|
}
|
|
|
|
float sin_input_generate(sin_input_generate_t* InputGenerate)
|
|
{
|
|
InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt);
|
|
InputGenerate->time += InputGenerate->DeltaT;
|
|
if(InputGenerate->time >= 20*(1/InputGenerate->frequency[InputGenerate->frequency_index]))
|
|
{
|
|
InputGenerate->time = 0;
|
|
InputGenerate->frequency_index += 1;
|
|
}
|
|
if(InputGenerate->frequency_index >= 64)
|
|
{
|
|
InputGenerate->input = 0;
|
|
}
|
|
else
|
|
InputGenerate->input = arm_sin_f32(2*PI*InputGenerate->frequency[InputGenerate->frequency_index]*InputGenerate->time);
|
|
//float input = arm_sin_f32(2*PI*frequency*time);
|
|
InputGenerate->input *= 2000;
|
|
|
|
return InputGenerate->input;
|
|
}
|
|
|
|
float step_input_generate(sin_input_generate_t* InputGenerate)
|
|
{
|
|
static int8_t forward_flag = 1;
|
|
InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt);
|
|
InputGenerate->time += InputGenerate->DeltaT;
|
|
if(InputGenerate->time >= 3)
|
|
{
|
|
if(forward_flag ==1) forward_flag = -1;
|
|
else if (forward_flag == -1) forward_flag = 1;
|
|
|
|
InputGenerate->time = 0;
|
|
}
|
|
return 60*forward_flag;
|
|
} |