From e2e32f20dfa4a24c7791cc9fe99a6c795749f7b9 Mon Sep 17 00:00:00 2001 From: zyx <1323655521@qq.com> Date: Tue, 7 May 2024 22:49:47 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E5=9B=BE=E4=BC=A0+=E9=99=90=E4=BD=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 3 +- application/cmd/robot_cmd.c | 8 +++-- application/robot_def.h | 7 ++++- application/to_stretch/to_stretch.c | 46 +++++++++++++++++++++++++---- engineering.jdebug.user | 46 ++++++++++++----------------- 5 files changed, 73 insertions(+), 37 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index eaec085..966dee4 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -45,7 +45,7 @@ static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数 static SuperCapInstance *cap; // 超级电容 -static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back +static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; float chassis_power = 0; static first_order_filter_type_t vx_filter; static first_order_filter_type_t vy_filter; @@ -111,7 +111,6 @@ void ChassisInit() first_order_filter_init(&vy_filter,0.002f,&chassis_y_order_filter); first_order_filter_init(&vx_filter,0.002f,&chassis_x_order_filter); - // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index e96c098..e52bfd3 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -128,8 +128,7 @@ static void RemoteControlSet() { { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; 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; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE; @@ -138,6 +137,11 @@ static void RemoteControlSet() { { 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;//0.0025f + to_stretch_cmd_send.tc += 0.025F * (float) rc_data[TEMP].rc.rocker_r_;//0.0025f + + // 图传限位 + if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE; + if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_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; diff --git a/application/robot_def.h b/application/robot_def.h index 586aee6..6de6d87 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -31,6 +31,8 @@ #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define TUCHUAN_MAX_ANGLE 180 // 图传电机最大角度 +#define TUCHUAN_MIN_ANGLE 0 // 图传电机最小角度 // 发射参数 #define ONE_BULLET_DELTA_ANGLE 1933.272 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 27.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49，增加为27 @@ -139,6 +141,7 @@ typedef struct // cmd发布的伸缩控制数据,由to_stretch订阅 typedef struct { + float tc; float ud; float fb; to_stretch_mode_e to_stretch_mode; @@ -174,7 +177,9 @@ typedef struct typedef struct { - + float gravity_feedforward; + float protract_x; //前伸距离 + float lift_z; //抬升高度 } To_stretch_Upload_Data_s; #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) diff --git a/application/to_stretch/to_stretch.c b/application/to_stretch/to_stretch.c index 96f5016..27d17f5 100644 --- a/application/to_stretch/to_stretch.c +++ b/application/to_stretch/to_stretch.c @@ -16,7 +16,7 @@ static Subscriber_t *to_stretch_sub; // 用于订阅伸缩的 static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv; // 伸缩接收到的控制命令 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, *tuchuan; float gravity_feedforward = 3000; float l_protract = 0,r_protract = 0; @@ -96,7 +96,6 @@ void To_stretchInit() { }, .motor_type = M3508, }; - // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 //上下 ud_config.can_init_config.tx_id = 5; ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; @@ -138,6 +137,39 @@ void To_stretchInit() { }; PIDInit(&lift_position_loop,&lift_pid_config); + // 图传电机 + Motor_Init_Config_s tuchuan_config = { + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 1, + }, + .controller_param_init_config = { + .angle_PID = { + .Kp = 20, + .Ki = 10, + .Kd = 0, + .MaxOut = 15000, + }, + .speed_PID = { + .Kp = 2.0f, + .Ki = 1.0f, + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 30000, + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = ANGLE_LOOP | SPEED_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = M2006 + }; + tuchuan = DJIMotorInit(&tuchuan_config); + 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)); @@ -177,6 +209,7 @@ void To_stretchTask() DJIMotorStop(motor_ru); DJIMotorStop(motor_ld); DJIMotorStop(motor_rd); + DJIMotorStop(tuchuan); } else { // 正常工作 @@ -184,9 +217,10 @@ void To_stretchTask() DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); + DJIMotorEnable(tuchuan); } - // 根据模式设定运动形式 + // 根据模式设定伸缩+图传运动形式 switch (to_stretch_cmd_recv.to_stretch_mode) { case TO_STRETCH_ZERO_FORCE: @@ -194,25 +228,27 @@ void To_stretchTask() DJIMotorStop(motor_ru); DJIMotorStop(motor_ld); DJIMotorStop(motor_rd); + DJIMotorStop(tuchuan); break; case TO_STRETCH_MODE: DJIMotorEnable(motor_lu); DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); + DJIMotorEnable(tuchuan); -// DJIMotorSetRef(motor_ld, 1000); -// DJIMotorSetRef(motor_ld, 1000); DJIMotorSetRef(motor_lu, ud_speed_set); DJIMotorSetRef(motor_ru, -ud_speed_set); DJIMotorSetRef(motor_ld, -fb_speed_set); DJIMotorSetRef(motor_rd, fb_speed_set); + DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc); break; case TO_STRETCH_KEEP: DJIMotorEnable(motor_lu); DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); + DJIMotorEnable(tuchuan); break; default: break; diff --git a/engineering.jdebug.user b/engineering.jdebug.user index eedf644..e67ff27 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,35 +1,27 @@ -GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f -GraphedExpression="(lift_position_loop).Measure", Color=#35792b -GraphedExpression="(lift_position_loop).Output", Color=#769dda -OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 -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 +OpenDocument="main.c", FilePath="D:/zhandui/cqdm/engineering/Src/main.c", Line=65 +OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/engineering/application/chassis/chassis.c", Line=94 +OpenDocument="to_stretch.c", FilePath="D:/zhandui/cqdm/engineering/application/to_stretch/to_stretch.c", Line=215 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=603, h=208, 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="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=470, 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="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=245, 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="Console", DockArea=BOTTOM, x=1, y=0, w=209, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=649, h=340, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=1206, h=264, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +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=649, h=716, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=948, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=1098, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1206, h=792, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;539", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="716;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="659;0" +OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=285, h=384, 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;770] -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;118;118;110;110;100;118;110] +TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;950] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[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="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"], ColWidths=[238;226;104;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] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[257;124;104;100] +TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;27] +WatchedExpression="motor_lf", 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 \ No newline at end of file +WatchedExpression="tuchuan", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="to_stretch_cmd_recv", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file From 04b3942c4d5dadb25a52ea0e9666a808c2a343de Mon Sep 17 00:00:00 2001 From: zyx <1323655521@qq.com> Date: Thu, 9 May 2024 21:33:08 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E5=9B=BE=E4=BC=A0+=E9=99=90=E4=BD=8D?= =?UTF-8?q?=EF=BC=8C=E6=8A=AC=E5=8D=87=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/cmd/robot_cmd.c | 19 ++++++++++++---- application/gimbal/gimbal.cpp | 12 +++++++++- application/robot.c | 8 +++---- application/robot_def.h | 5 +---- application/to_stretch/to_stretch.c | 27 ++++++++++++---------- engineering.jdebug.user | 35 +++++++++++++---------------- modules/motor/DMmotor/dmmotor.c | 2 +- modules/motor/DMmotor/dmmotor.h | 2 +- 8 files changed, 63 insertions(+), 47 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index b583564..c909dbb 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -116,6 +116,13 @@ static void CalcOffsetAngle() { #endif } +static void death_check() +{ + if(referee_data->GameRobotState.remain_HP) + { + gimbal_cmd_send.MotorEnableFlag=5; + } +} /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -128,7 +135,7 @@ static void update_ui_data() { // 出招表 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; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP; @@ -137,8 +144,11 @@ static void RemoteControlSet() { chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转 - - } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩 + to_stretch_cmd_send.tc += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//图传 + // 图传限位 + if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE; + if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE; + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动 { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE; @@ -149,13 +159,14 @@ static void RemoteControlSet() { to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l_;//抬升 to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//前伸 + //伸缩限位待添加 // 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 (switch_is_down(rc_data[TEMP].rc.switch_right)) { + if (switch_is_up(rc_data[TEMP].rc.switch_right)) { // 左侧开关状态为[下],前三轴 if(switch_is_down(rc_data[TEMP].rc.switch_left)) { diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index eaa7f37..53658c2 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -220,12 +220,22 @@ void GimbalInit() }; PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config); - gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } +//static void DMMotroEnable() +//{ +// if(gimbal_cmd_recv.MotorEnableFlag) +// { +// DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor); +// DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor); +// DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor); +// DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor ); +// DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor ); +// } +//} /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() diff --git a/application/robot.c b/application/robot.c index 4619b61..ee1cf10 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/application/robot_def.h b/application/robot_def.h index 577f976..df23ee0 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -35,9 +35,6 @@ #define YAW 60 - - - // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 @@ -150,7 +147,7 @@ typedef struct float diff_pitch; //差速器pitch float diff_roll; //差速器roll float chassis_rotate_wz; - + uint8_t MotorEnableFlag; gimbal_mode_e gimbal_mode; } Gimbal_Ctrl_Cmd_s; diff --git a/application/to_stretch/to_stretch.c b/application/to_stretch/to_stretch.c index 27d17f5..4cd45f7 100644 --- a/application/to_stretch/to_stretch.c +++ b/application/to_stretch/to_stretch.c @@ -25,6 +25,7 @@ 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; +float kp=4,ki=1,kd=0;//调试用 PIDInstance protract_position_loop;//前伸位置环 PIDInstance lift_position_loop;//抬升位置环 @@ -45,24 +46,23 @@ void To_stretchInit() { .MaxOut = 500, }, .speed_PID = { - .Kp = 2, + .Kp = 4, .Ki = 1, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 2500, - .MaxOut = 5000, + .MaxOut = 13000, }, .current_feedforward_ptr = &gravity_feedforward, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = SPEED_LOOP,//ANGLE_LOOP, - .close_loop_type = SPEED_LOOP,//SPEED_LOOP | ANGLE_LOOP, + .outer_loop_type = SPEED_LOOP, + .close_loop_type = SPEED_LOOP, .feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = M3508, - }; // 前后 Motor_Init_Config_s fb_config = { @@ -123,7 +123,7 @@ void To_stretchInit() { .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, - .MaxOut = 20000, + .MaxOut = 15000, }; PIDInit(&protract_position_loop,&protract_pid_config); @@ -133,14 +133,14 @@ void To_stretchInit() { .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, - .MaxOut = 20000, + .MaxOut = 15000, }; PIDInit(&lift_position_loop,&lift_pid_config); // 图传电机 Motor_Init_Config_s tuchuan_config = { .can_init_config = { - .can_handle = &hcan2, + .can_handle = &hcan1, .tx_id = 1, }, .controller_param_init_config = { @@ -148,7 +148,7 @@ void To_stretchInit() { .Kp = 20, .Ki = 10, .Kd = 0, - .MaxOut = 15000, + .MaxOut = 5000, }, .speed_PID = { .Kp = 2.0f, @@ -156,7 +156,7 @@ void To_stretchInit() { .Kd = 0, .Improve = PID_Integral_Limit, .IntegralLimit = 5000, - .MaxOut = 30000, + .MaxOut = 5000, }, }, .controller_setting_init_config = { @@ -178,6 +178,10 @@ void To_stretchInit() { /* 机器人伸缩控制核心任务 */ void To_stretchTask() { + motor_lu->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp; + motor_lu->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki; + motor_lu->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd; + static uint8_t init_flag = FALSE; if(init_flag == FALSE) { @@ -235,13 +239,11 @@ void To_stretchTask() DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); - DJIMotorEnable(tuchuan); DJIMotorSetRef(motor_lu, ud_speed_set); DJIMotorSetRef(motor_ru, -ud_speed_set); DJIMotorSetRef(motor_ld, -fb_speed_set); DJIMotorSetRef(motor_rd, fb_speed_set); - DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc); break; case TO_STRETCH_KEEP: DJIMotorEnable(motor_lu); @@ -249,6 +251,7 @@ void To_stretchTask() DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); DJIMotorEnable(tuchuan); + DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc); break; default: break; diff --git a/engineering.jdebug.user b/engineering.jdebug.user index eedf644..a062d3f 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,30 +1,25 @@ -GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f -GraphedExpression="(lift_position_loop).Measure", Color=#35792b -GraphedExpression="(lift_position_loop).Output", Color=#769dda -OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 -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 +OpenDocument="gimbal.cpp", FilePath="D:/zhandui/cqdm/engineering/application/gimbal/gimbal.cpp", Line=234 +OpenDocument="main.c", FilePath="D:/zhandui/cqdm/engineering/Src/main.c", Line=48 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=603, h=208, 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="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=470, 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="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=245, 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="Console", DockArea=BOTTOM, x=1, y=0, w=209, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=603, h=387, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=1011, h=229, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=523, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=788, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1375, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=383, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=1011, h=946, 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="784;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="800;0" +OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=276, h=245, 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;770] -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;118;118;110;110;100;118;110] +TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;1246] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[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="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;118;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] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[257;124;104;100] +TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;26] 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 diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 4256e17..0915e60 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -25,7 +25,7 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; } -static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) +void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) { memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index 68c8fe0..3d26871 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -68,7 +68,7 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref); void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type); void DMMotorEnable(DMMotorInstance *motor); - +void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor); void DMMotorStop(DMMotorInstance *motor); void DMMotorCaliEncoder(DMMotorInstance *motor); void DMMotorControlInit();