From 4f8100efbcab24ef113f0fa8e33943520773aa06 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Wed, 6 Dec 2023 20:22:15 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9EGM6020=E7=94=B5=E6=B5=81?= =?UTF-8?q?=E7=8E=AF=E6=8E=A7=E5=88=B6=E6=94=AF=E6=8C=81=20=20=E9=87=8D?= =?UTF-8?q?=E5=8A=9B=E8=A1=A5=E5=81=BF=E5=8A=9B=E7=9F=A9=E8=AE=A1=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/gimbal/gimbal.c | 167 ++++++++++++++++++----------- modules/motor/DJImotor/dji_motor.c | 41 +++++-- modules/motor/DJImotor/dji_motor.h | 1 + modules/motor/motor_def.h | 9 ++ 4 files changed, 145 insertions(+), 73 deletions(-) diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5063698..9b6258f 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -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); + } } \ No newline at end of file diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index e9345cc..c0d9935 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -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]) { diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h index bd07c1a..58ee45c 100644 --- a/modules/motor/DJImotor/dji_motor.h +++ b/modules/motor/DJImotor/dji_motor.h @@ -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; diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index d73476e..72c42fa 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -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