zyx
/
engineering
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

抬升 前伸 速度位置双环 降低上层四个3508反馈频率至500HZ

This commit is contained in:
宋家齐 2024-04-26 17:35:17 +08:00
parent e07c2ee99f
commit 47689e75dc
6 changed files with 132 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
application/cmd/robot_cmd.c
View File

@ -124,25 +124,23 @@ static void update_ui_data() {
} }
static void RemoteControlSet() { static void RemoteControlSet() {
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘运动 if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘运动,伸缩保持不动
{ {
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_ZERO_FORCE; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP;
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘不动，伸缩 } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘不动，伸缩
{ {
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
} }
if (switch_is_mid(rc_data[TEMP].rc.switch_left) && (switch_is_mid(rc_data[TEMP].rc.switch_right))) if (switch_is_mid(rc_data[TEMP].rc.switch_left) && (switch_is_mid(rc_data[TEMP].rc.switch_right)))
{ {
to_stretch_cmd_send.ud -= 0.0025f * (float) rc_data[TEMP].rc.rocker_l1; to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//0.0025f
to_stretch_cmd_send.fb -= 0.0025f * (float) rc_data[TEMP].rc.rocker_r1; to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_r1;//0.0025f
//伸缩限位待添加 //伸缩限位待添加
// if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE; // if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
// if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; // if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
} }
// 左侧开关状态为[下],视觉模式 // 左侧开关状态为[下],视觉模式
if (switch_is_down(rc_data[TEMP].rc.switch_left)) { if (switch_is_down(rc_data[TEMP].rc.switch_left)) {

1
application/robot_def.h
View File

@ -102,6 +102,7 @@ typedef enum
{ {
TO_STRETCH_ZERO_FORCE = 0, // 电流零输入 TO_STRETCH_ZERO_FORCE = 0, // 电流零输入
TO_STRETCH_MODE, // 自由伸缩 TO_STRETCH_MODE, // 自由伸缩
TO_STRETCH_KEEP, // 保持不动
} to_stretch_mode_e; } to_stretch_mode_e;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */

111
application/to_stretch/to_stretch.c
View File

@ -18,6 +18,17 @@ static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反
static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd; static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd;
float gravity_feedforward = 3000;
float l_protract = 0,r_protract = 0;
float protract_x = 0; //前伸距离
float l_lift = 0,r_lift = 0;
float lift_z = 0; //抬升高度
float ld_offset = 0,rd_offset = 0;
float lu_offset = 0,ru_offset = 0;
PIDInstance protract_position_loop;//前伸位置环
PIDInstance lift_position_loop;//抬升位置环
void To_stretchInit() { void To_stretchInit() {
// 上下 // 上下
Motor_Init_Config_s ud_config = { Motor_Init_Config_s ud_config = {
@ -34,23 +45,24 @@ void To_stretchInit() {
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp = 0, .Kp = 2,
.Ki = 0, .Ki = 1,
.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 = 2500, .IntegralLimit = 2500,
.MaxOut = 20000, .MaxOut = 5000,
}, },
.current_feedforward_ptr = &gravity_feedforward,
}, },
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = SPEED_LOOP,//ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP,//SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE, .feedforward_flag = CURRENT_FEEDFORWARD,
.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
}, },
.motor_type = M3508, .motor_type = M3508,
}; };
// 前后 // 前后
Motor_Init_Config_s fb_config = { Motor_Init_Config_s fb_config = {
@ -59,50 +71,73 @@ void To_stretchInit() {
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 0.01f, .Kp = 10.0f,
.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 = 5000,
}, },
.speed_PID = { .speed_PID = {
.Kp = 10, .Kp = 2,
.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 = 2500, .IntegralLimit = 2500,
.MaxOut = 20000, .MaxOut = 5000,
}, },
.other_angle_feedback_ptr = &protract_x,
}, },
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = OTHER_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
}, },
.motor_type = M3508, .motor_type = M3508,
}; };
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
//上下
ud_config.can_init_config.tx_id = 5; ud_config.can_init_config.tx_id = 5;
ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL;
motor_lu = DJIMotorInit(&ud_config); motor_lu = DJIMotorInit(&ud_config);
ud_config.can_init_config.tx_id = 6; ud_config.can_init_config.tx_id = 6;
ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE;
motor_ru = DJIMotorInit(&ud_config); motor_ru = DJIMotorInit(&ud_config);
//前后
fb_config.can_init_config.tx_id = 7; fb_config.can_init_config.tx_id = 7;
fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL;
motor_ld = DJIMotorInit(&fb_config); motor_ld = DJIMotorInit(&fb_config);
fb_config.can_init_config.tx_id = 8; fb_config.can_init_config.tx_id = 8;
fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE;
motor_rd = DJIMotorInit(&fb_config); motor_rd = DJIMotorInit(&fb_config);
PID_Init_Config_s protract_pid_config = {
.Kp = 2500.0f,
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 20000,
};
PIDInit(&protract_position_loop,&protract_pid_config);
PID_Init_Config_s lift_pid_config = {
.Kp = 2500.0f,
.Ki = 0,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 20000,
};
PIDInit(&lift_position_loop,&lift_pid_config);
to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s)); to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s)); to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s));
@ -111,9 +146,31 @@ void To_stretchInit() {
/* 机器人伸缩控制核心任务 */ /* 机器人伸缩控制核心任务 */
void To_stretchTask() void To_stretchTask()
{ {
static uint8_t init_flag = FALSE;
if(init_flag == FALSE)
{
ld_offset = motor_ld->measure.total_angle ;
rd_offset = motor_rd->measure.total_angle ;
lu_offset = motor_lu->measure.total_angle ;
ru_offset = motor_ru->measure.total_angle ;
init_flag = TRUE;
}
l_protract = -(motor_ld->measure.total_angle - ld_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm
r_protract = (motor_rd->measure.total_angle - rd_offset) / 19.0f * DEGREE_2_RAD * 20.0f;
l_lift = (motor_lu->measure.total_angle - lu_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm
r_lift = -(motor_ru->measure.total_angle - ru_offset) / 19.0f * DEGREE_2_RAD * 20.0f;
protract_x = (l_protract+r_protract)/2;
lift_z = (l_lift+r_lift)/2;
SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv); SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv);
float fb_speed_set = PIDCalculate(&protract_position_loop,protract_x,to_stretch_cmd_recv.fb);
float ud_speed_set = PIDCalculate(&lift_position_loop,lift_z,to_stretch_cmd_recv.ud);
if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE) if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止 { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
DJIMotorStop(motor_lu); DJIMotorStop(motor_lu);
@ -129,7 +186,7 @@ void To_stretchTask()
DJIMotorEnable(motor_rd); DJIMotorEnable(motor_rd);
} }
// 根据控制模式设定旋转速度 // 根据模式设定运动形式
switch (to_stretch_cmd_recv.to_stretch_mode) switch (to_stretch_cmd_recv.to_stretch_mode)
{ {
case TO_STRETCH_ZERO_FORCE: case TO_STRETCH_ZERO_FORCE:
@ -144,10 +201,18 @@ void To_stretchTask()
DJIMotorEnable(motor_ld); DJIMotorEnable(motor_ld);
DJIMotorEnable(motor_rd); DJIMotorEnable(motor_rd);
DJIMotorSetRef(motor_lu, to_stretch_cmd_recv.ud); // DJIMotorSetRef(motor_ld, 1000);
DJIMotorSetRef(motor_ru, to_stretch_cmd_recv.ud); // DJIMotorSetRef(motor_ld, 1000);
DJIMotorSetRef(motor_ld, to_stretch_cmd_recv.fb); DJIMotorSetRef(motor_lu, ud_speed_set);
DJIMotorSetRef(motor_rd, to_stretch_cmd_recv.fb); DJIMotorSetRef(motor_ru, -ud_speed_set);
DJIMotorSetRef(motor_ld, -fb_speed_set);
DJIMotorSetRef(motor_rd, fb_speed_set);
break;
case TO_STRETCH_KEEP:
DJIMotorEnable(motor_lu);
DJIMotorEnable(motor_ru);
DJIMotorEnable(motor_ld);
DJIMotorEnable(motor_rd);
break; break;
default: default:
break; break;

2
engineering.jdebug
View File

@ -33,6 +33,8 @@ void OnProjectLoad (void) {
// //
// User settings // User settings
// //
Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ);
Project.SetOSPlugin ("FreeRTOSPlugin_CM4");
File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf"); File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf");
} }

49
engineering.jdebug.user
View File

@ -1,26 +1,35 @@
OpenDocument="stm32f4xx_it.c", FilePath="D:/zhandui/cqdm/engineering/Src/stm32f4xx_it.c", Line=103 GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/zhandui/cqdm/engineering/Startup/startup_stm32f407ighx.s", Line=47 GraphedExpression="(lift_position_loop).Measure", Color=#35792b
OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/zhandui/cqdm/engineering/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=187 GraphedExpression="(lift_position_loop).Output", Color=#769dda
OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/engineering/application/chassis/chassis.c", Line=94 OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69
OpenDocument="main.c", FilePath="D:/zhandui/cqdm/engineering/Src/main.c", Line=71 OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0
OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3399
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51
OpenDocument="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0
OpenToolbar="Debug", Floating=0, x=0, y=0 OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=649, h=297, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=603, h=208, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=935, h=233, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=398, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=546, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=470, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=649, h=739, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1046, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1434, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=291, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1011, h=679, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;472", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="717;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="238;0"
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=935, h=803, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="708;-69", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="717;-69", CodeGraphLegendShown=1, CodeGraphLegendPosition="659;0" OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=209, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=286, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" 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;950] TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;770]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100] TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (lift_position_loop).Ref";" (lift_position_loop).Measure";" (lift_position_loop).Output"], ColWidths=[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;100;100;100;100;110;126;126] TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;118;118;110;110;100;118;110]
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[238;226;104;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="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;659] TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
WatchedExpression="motor_lf", RefreshRate=2, Window=Watched Data 1 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[257;124;104;100]
WatchedExpression="chassis_cmd_recv", RefreshRate=2, Window=Watched Data 1 WatchedExpression="chassis_cmd_recv", RefreshRate=2, Window=Watched Data 1
WatchedExpression="motor_ld", RefreshRate=2, Window=Watched Data 1
WatchedExpression="motor_rd", RefreshRate=2, Window=Watched Data 1
WatchedExpression="gravity_feedforward", RefreshRate=2, Window=Watched Data 1
WatchedExpression="motor_lu", Window=Watched Data 1
WatchedExpression="motor_ru", Window=Watched Data 1
WatchedExpression="protract_position_loop", RefreshRate=2, Window=Watched Data 1
WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1

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

@ -210,7 +210,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
Daemon_Init_Config_s daemon_config = { Daemon_Init_Config_s daemon_config = {
.callback = DJIMotorLostCallback, .callback = DJIMotorLostCallback,
.owner_id = instance, .owner_id = instance,
.reload_count = 2, // 20ms未收到数据则丢失 .reload_count = 100, // 20ms未收到数据则丢失
}; };
instance->daemon = DaemonRegister(&daemon_config); instance->daemon = DaemonRegister(&daemon_config);
@ -300,7 +300,13 @@ void DJIMotorControl() {
if (motor_setting->speed_feedback_source == OTHER_FEED) if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr; pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED else // MOTOR_FEED
pid_measure = measure->speed_aps; {
if(motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_measure = - measure->speed_aps;
else //NORMAL
pid_measure = measure->speed_aps;
}
// 更新pid_ref进入下一个环 // 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
} }