HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

新增GM6020电流环控制支持 重力补偿力矩计算

This commit is contained in:
宋家齐 2023-12-06 20:22:15 +08:00
parent e25cbdf2a3
commit 4f8100efbc
4 changed files with 145 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
application/gimbal/gimbal.c
View File

@ -21,6 +21,7 @@ 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,
@ -55,46 +56,54 @@ void GimbalInit()
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020};
.motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL};
// PITCH
Motor_Init_Config_s pitch_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 4,
.tx_id = 2,
},
.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]),
// .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]),
.current_PID = {
.Kp = 1, // 10
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 30000,
}
},
.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,
.outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
// pitch_motor = DJIMotorInit(&pitch_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));
@ -109,55 +118,83 @@ void GimbalTask()
// @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;
}
// 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电机角度反馈计算出当前配重下的重力矩
// ...
static uint32_t cnt;
static float time;
DJIMotorEnable(pitch_motor);
float deltaT=DWT_GetDeltaT(&cnt);
time += deltaT;
float theta = pitch_motor->measure.angle_single_round - 6200 * ECD_ANGLE_COEF_DJI;
//float input = arm_sin_f32(2*PI*10*time);
float gravity_feed = 3800*arm_cos_f32(theta/180*PI);
DJIMotorSetRef(pitch_motor,gravity_feed);
float vofa_send_data[4];
vofa_send_data[0]=yaw_motor->motor_controller.speed_PID.Ref;
vofa_send_data[1]=yaw_motor->motor_controller.speed_PID.Measure;
vofa_send_data[2]=yaw_motor->motor_controller.angle_PID.Ref;
vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure;
// vofa_send_data[0]=yaw_motor->motor_controller.speed_PID.Ref;
// vofa_send_data[1]=yaw_motor->motor_controller.speed_PID.Measure;
// vofa_send_data[2]=yaw_motor->motor_controller.angle_PID.Ref;
// vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure;
// vofa_justfloat_output(vofa_send_data,16,&huart1);
vofa_send_data[0] = pitch_motor->motor_controller.pid_ref;
vofa_send_data[1] = pitch_motor->measure.real_current;
vofa_send_data[2] = theta;
vofa_send_data[3] = gravity_feed;
vofa_justfloat_output(vofa_send_data,16,&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;
// 推送消息
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);
}
}

41
modules/motor/DJImotor/dji_motor.c
View File

@ -19,14 +19,19 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/
static CANInstance sender_assignment[6] = {
static CANInstance sender_assignment[10] = {
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
};
[6] = {.can_handle = &hcan1, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[7] = {.can_handle = &hcan1, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[8] = {.can_handle = &hcan2, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[9] = {.can_handle = &hcan2, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
};
/**
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
@ -79,15 +84,31 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
break;
case GM6020:
if (motor_id < 4)
if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 6 : 8;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 7 : 9;
}
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
}
}
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
@ -165,6 +186,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
// motor controller init 电机控制器初始化
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
@ -286,6 +309,8 @@ void DJIMotorControl()
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
}
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1;
@ -304,7 +329,7 @@ void DJIMotorControl()
}
// 遍历flag,检查是否要发送这一帧报文
for (size_t i = 0; i < 6; ++i)
for (size_t i = 0; i < 10; ++i)
{
if (sender_enable_flag[i])
{

1
modules/motor/DJImotor/dji_motor.h
View File

@ -58,6 +58,7 @@ typedef struct
uint8_t message_num;
Motor_Type_e motor_type; // 电机类型
Motor_Control_Type_e motor_control_type; //电机控制方式
Motor_Working_Type_e stop_flag; // 启停标志
DaemonInstance* daemon;

9
modules/motor/motor_def.h
View File

@ -108,6 +108,14 @@ typedef enum
HT04,
} Motor_Type_e;
/* 电机控制方式枚举 */
typedef enum
{
CONTROL_TYPE_NONE = 0,
CURRENT_CONTROL,
VOLTAGE_CONTROL,
} Motor_Control_Type_e;
/**
* @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针
* 如果不需要某个控制环,可以不设置对应的pid config
@ -133,6 +141,7 @@ typedef struct
Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type;
CAN_Init_Config_s can_init_config;
Motor_Control_Type_e motor_control_type;
} Motor_Init_Config_s;
#endif // !MOTOR_DEF_H