新增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

View File

@ -21,6 +21,7 @@ void GimbalInit()
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW // YAW
Motor_Init_Config_s yaw_config = { Motor_Init_Config_s yaw_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan1, .can_handle = &hcan1,
.tx_id = 1, .tx_id = 1,
@ -55,46 +56,54 @@ void GimbalInit()
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = GM6020}; .motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL};
// PITCH // PITCH
Motor_Init_Config_s pitch_config = { Motor_Init_Config_s pitch_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
.tx_id = 4, .tx_id = 2,
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { // .angle_PID = {
.Kp = 10, // 10 // .Kp = 10, // 10
.Ki = 0, // .Ki = 0,
.Kd = 0, // .Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, // .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100, // .IntegralLimit = 100,
.MaxOut = 500, // .MaxOut = 500,
}, // },
.speed_PID = { // .speed_PID = {
.Kp = 50, // 50 // .Kp = 50, // 50
.Ki = 350, // 350 // .Ki = 350, // 350
.Kd = 0, // 0 // .Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, // .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500, // .IntegralLimit = 2500,
.MaxOut = 20000, // .MaxOut = 20000,
}, // },
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), // .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 = { .controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED, .outer_loop_type = OPEN_LOOP,
.speed_feedback_source = OTHER_FEED, .close_loop_type = OPEN_LOOP,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = GM6020, .motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL
}; };
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config); 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_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
@ -109,55 +118,83 @@ void GimbalTask()
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
switch (gimbal_cmd_recv.gimbal_mode) // switch (gimbal_cmd_recv.gimbal_mode)
{ // {
// 停止 // // 停止
case GIMBAL_ZERO_FORCE: // case GIMBAL_ZERO_FORCE:
DJIMotorStop(yaw_motor); // DJIMotorStop(yaw_motor);
DJIMotorStop(pitch_motor); // DJIMotorStop(pitch_motor);
break; // break;
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 // // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
case GIMBAL_GYRO_MODE: // 后续只保留此模式 // case GIMBAL_GYRO_MODE: // 后续只保留此模式
DJIMotorEnable(yaw_motor); // DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor); // DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); // DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); // DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); // DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); // DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 // DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); //// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break; // break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 // // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) // case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
DJIMotorEnable(yaw_motor); // DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor); // DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); // DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); // DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); // DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); // DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 // DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); //// DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break; // break;
default: // default:
break; // break;
} // }
// 在合适的地方添加pitch重力补偿前馈力矩 // 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/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]; float vofa_send_data[4];
vofa_send_data[0]=yaw_motor->motor_controller.speed_PID.Ref; // 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[1]=yaw_motor->motor_controller.speed_PID.Measure;
vofa_send_data[2]=yaw_motor->motor_controller.angle_PID.Ref; // 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[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); vofa_justfloat_output(vofa_send_data,16,&huart1);
// 设置反馈数据,主要是imu和yaw的ecd // 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; 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.angle_single_round;
// 推送消息 // 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); 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);
}
} }

View File

@ -19,14 +19,19 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]: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}}, [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}}, [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}}, [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}}, [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}}, [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}}, [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 6sender_assignment中的标志位,,DJIMotorControl()使 * @brief 6sender_assignment中的标志位,,DJIMotorControl()使
@ -79,15 +84,31 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
break; break;
case GM6020: case GM6020:
if (motor_id < 4) if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe
{ {
motor_send_num = motor_id; if (motor_id < 4)
motor_grouping = config->can_handle == &hcan1 ? 0 : 3; {
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 else
{ {
motor_send_num = motor_id - 4; if (motor_id < 4)
motor_grouping = config->can_handle == &hcan1 ? 2 : 5; {
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,进行分组设置 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_type = config->motor_type; // 6020 or 2006 or 3508
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等 instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
// motor controller init 电机控制器初始化 // motor controller init 电机控制器初始化
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID); 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); 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); pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
} }
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1; pid_ref *= -1;
@ -304,7 +329,7 @@ void DJIMotorControl()
} }
// 遍历flag,检查是否要发送这一帧报文 // 遍历flag,检查是否要发送这一帧报文
for (size_t i = 0; i < 6; ++i) for (size_t i = 0; i < 10; ++i)
{ {
if (sender_enable_flag[i]) if (sender_enable_flag[i])
{ {

View File

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

View File

@ -108,6 +108,14 @@ typedef enum
HT04, HT04,
} Motor_Type_e; } Motor_Type_e;
/* 电机控制方式枚举 */
typedef enum
{
CONTROL_TYPE_NONE = 0,
CURRENT_CONTROL,
VOLTAGE_CONTROL,
} Motor_Control_Type_e;
/** /**
* @brief ,PID的配置以及两个反馈数据来源指针 * @brief ,PID的配置以及两个反馈数据来源指针
* ,pid config * ,pid config
@ -133,6 +141,7 @@ typedef struct
Motor_Control_Setting_s controller_setting_init_config; Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type; Motor_Type_e motor_type;
CAN_Init_Config_s can_init_config; CAN_Init_Config_s can_init_config;
Motor_Control_Type_e motor_control_type;
} Motor_Init_Config_s; } Motor_Init_Config_s;
#endif // !MOTOR_DEF_H #endif // !MOTOR_DEF_H