From 2de0ed56cabb624c9e5120e8fcc01fc8055dad1f Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Fri, 10 May 2024 16:05:21 +0800 Subject: [PATCH] =?UTF-8?q?pitch=E8=BD=B4=E5=8F=8C6006?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/gimbal/gimbal.cpp | 93 ++++++++++++++++++++--------------- application/robot.c | 8 +-- engineering.jdebug.user | 29 ++++++----- 3 files changed, 74 insertions(+), 56 deletions(-) diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index 6abbfb8..a0a1a07 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -17,14 +17,16 @@ extern "C" { #include "robotics.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 -static DMMotorInstance *yaw_motor, *pitch_motor, *roll_motor; +static DMMotorInstance *yaw_motor, *pitch_motor_l,*pitch_motor_r, *roll_motor; static DMMotorInstance *diff_r_motor,*diff_l_motor; +//pitch轴双6006 双环pid算出力矩 一人一半 +static PIDInstance pitch_spd_loop,pitch_angle_loop; static PIDInstance diff_pitch_loop,diff_roll_loop; static PIDInstance diff_pitch_spd_loop,diff_roll_spd_loop; float arm_gravity_feedforward = 0; -float GRAVITY_COMP = 5.5; +float GRAVITY_COMP = 6.0; static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 @@ -100,40 +102,47 @@ void GimbalInit() Motor_Init_Config_s pitch_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &pitch_spd, - .current_feedforward_ptr = &arm_gravity_feedforward, - .speed_PID = { - .Kp = 2.5f, - .Ki = 0.8f, - .Kd = 0.0f, - .MaxOut = 10, - .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), - .IntegralLimit = 10.0F, - }, - .angle_PID = { - .Kp = 8.0f, - .Ki = 0.0f, - .Kd = 0.0f, - .MaxOut = 10, - .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), - .IntegralLimit = 10.0F, - }, + //.current_feedforward_ptr = &arm_gravity_feedforward, }, .controller_setting_init_config = { - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), +// .outer_loop_type = ANGLE_LOOP, +// .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), + .outer_loop_type = OPEN_LOOP, + .close_loop_type = OPEN_LOOP, .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = OTHER_FEED, - .feedforward_flag = CURRENT_FEEDFORWARD, + //.feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = DM6006, }; pitch_motor_config.can_init_config.can_handle = &hcan2; - pitch_motor_config.can_init_config.tx_id = 3; - pitch_motor_config.can_init_config.rx_id = 4; - pitch_motor = DMMotorInit(&pitch_motor_config); + pitch_motor_config.can_init_config.tx_id = 0x03; + pitch_motor_config.can_init_config.rx_id = 0x04; + pitch_motor_l = DMMotorInit(&pitch_motor_config); + pitch_motor_config.can_init_config.tx_id = 0x13; + pitch_motor_config.can_init_config.rx_id = 0x14; + pitch_motor_r = DMMotorInit(&pitch_motor_config); + PID_Init_Config_s pitch_spd_config= { + .Kp = 2.5f, + .Ki = 0.8f, + .Kd = 0.02f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 100, + }; + PID_Init_Config_s pitch_angle_config= { + .Kp = 12.0f, + .Ki = 0, + .Kd = 0, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 100, + }; + + PIDInit(&pitch_spd_loop,&pitch_spd_config); + PIDInit(&pitch_angle_loop,&pitch_angle_config); - //@todo：roll轴机械固定不牢 待细调 Motor_Init_Config_s roll_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &roll_spd, @@ -280,9 +289,9 @@ void GimbalTask() /* 控制参数计算 ------------------------------------------------------------------------*/ //大臂重力补偿力矩 - arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position); + arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position); //电机速度滤波 - first_order_filter_cali(&pitch_spd_filter,pitch_motor->measure.velocity); + first_order_filter_cali(&pitch_spd_filter,pitch_motor_l->measure.velocity); pitch_spd = pitch_spd_filter.out; first_order_filter_cali(&yaw_spd_filter,yaw_motor->measure.velocity); yaw_spd = yaw_spd_filter.out; @@ -297,20 +306,19 @@ void GimbalTask() float diff_pitch_angle = (diff_r_motor->measure.position + (-diff_l_motor->measure.position))/2; float diff_roll_angle = (diff_r_motor->measure.position - (-diff_l_motor->measure.position))/2 * 18/52; - float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); - float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); + float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); + float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); -// float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2; -// float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52; -// float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out); -// float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out); + float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out; + float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out; - float r_speed_set = pitch_angle_out + roll_angle_out; - float l_speed_set = pitch_angle_out - roll_angle_out; + //pitch轴双环PID + float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD); + float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward; //正运动学 arm_q[0] = yaw_motor->measure.position; - arm_q[1] = pitch_motor->measure.position; + arm_q[1] = pitch_motor_l->measure.position; arm_q[2] = roll_motor->measure.position; arm_q[3] = diff_pitch_angle; arm_q[4] = diff_roll_angle; @@ -327,7 +335,8 @@ void GimbalTask() // 停止 case GIMBAL_ZERO_FORCE: DMMotorStop(yaw_motor); - DMMotorStop(pitch_motor); + DMMotorStop(pitch_motor_l); + DMMotorStop(pitch_motor_r); DMMotorStop(roll_motor); DMMotorStop(diff_r_motor); DMMotorStop(diff_l_motor); @@ -336,9 +345,13 @@ void GimbalTask() // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 DMMotorEnable(yaw_motor); - DMMotorEnable(pitch_motor); + DMMotorEnable(pitch_motor_l); + DMMotorEnable(pitch_motor_r); DMMotorEnable(roll_motor); - DMMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD); + + DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2); + DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2); + //DMMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD); DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD); DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD); diff --git a/application/robot.c b/application/robot.c index ee1cf10..4619b61 100644 --- a/application/robot.c +++ b/application/robot.c @@ -31,8 +31,8 @@ void RobotInit() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDInit(); - //GimbalInit(); - To_stretchInit(); + GimbalInit(); + //To_stretchInit(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) @@ -49,8 +49,8 @@ void RobotTask() { #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDTask(); - //GimbalTask(); - To_stretchTask(); + GimbalTask(); + //To_stretchTask(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) diff --git a/engineering.jdebug.user b/engineering.jdebug.user index eba6cf5..b4869ce 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,35 +1,37 @@ -Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:265:1, State=BP_STATE_DISABLED -GraphedExpression="(diff_pitch_loop).Measure", Color=#e56a6f -GraphedExpression="(diff_pitch_loop).Ref", Color=#35792b -OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=72 +Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:269:5, State=BP_STATE_DISABLED +GraphedExpression="(pitch_angle_loop).Ref", Color=#e56a6f +GraphedExpression="(pitch_angle_loop).Measure", Color=#35792b +GraphedExpression="(pitch_spd_loop).Ref", Color=#769dda +GraphedExpression="(pitch_spd_loop).Measure", Color=#b14f0d +OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=13 OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3422 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=0 OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107 OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51 +OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=315 -OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=5 OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1255 OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0 OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412 OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=176, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=184, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=401, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C -OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=465, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=457, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1041, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=265, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=531, h=642, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="70;20", CodeGraphLegendShown=0, CodeGraphLegendPosition="80;20" +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=531, h=642, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="493;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="507;0" OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=210, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (diff_pitch_loop).Measure";" (diff_pitch_loop).Ref"], ColWidths=[100;100;100;100] -TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;100;100;100;100;100;107;107] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (pitch_angle_loop).Ref";" (pitch_angle_loop).Measure";" (pitch_spd_loop).Ref";" (pitch_spd_loop).Measure"], ColWidths=[100;100;100;100;100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;126;102;102;118;100;118;110] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;124;104;117;100] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;124;104;129;100] TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351] WatchedExpression="diff_r_motor", RefreshRate=2, Window=Watched Data 1 @@ -50,4 +52,7 @@ WatchedExpression="(fk_p).data_", RefreshRate=2, Window=Watched Data 1 WatchedExpression="(fk_rpy).data_", RefreshRate=2, Window=Watched Data 1 WatchedExpression="arm_q", RefreshRate=5, Window=Watched Data 1 WatchedExpression="ik_q.data_", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="pitch_motor", Window=Watched Data 1 \ No newline at end of file +WatchedExpression="pitch_motor", Window=Watched Data 1 +WatchedExpression="pitch_motor_l", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="pitch_spd_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="pitch_angle_loop", Window=Watched Data 1 \ No newline at end of file