pitch轴双6006
This commit is contained in:
parent
30ca291a68
commit
2de0ed56ca
|
|
@ -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_Improvement_e>(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_Improvement_e>(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<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
||||
// .outer_loop_type = ANGLE_LOOP,
|
||||
// .close_loop_type = static_cast<Closeloop_Type_e>(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_Improvement_e>(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_Improvement_e>(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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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
|
||||
Loading…
Reference in New Issue