diff --git a/Ozone/ozone.jdebug b/Ozone/ozone.jdebug new file mode 100644 index 0000000..64e9d54 --- /dev/null +++ b/Ozone/ozone.jdebug @@ -0,0 +1,361 @@ +/********************************************************************* +* (c) SEGGER Microcontroller GmbH * +* The Embedded Experts * +* www.segger.com * +********************************************************************** + +File : +Created : 09. Mar 2026 16:55 +Ozone Version : V3.40b +*/ + +/********************************************************************* +* +* OnProjectLoad +* +* Function description +* Project load routine. Required. +* +********************************************************************** +*/ +void OnProjectLoad (void) { + // + // Dialog-generated settings + // + Project.AddPathSubstitute ("D:/STM32CubeMX/103/Er(Sentry)/Ozone", "$(ProjectDir)"); + Project.AddPathSubstitute ("d:/stm32cubemx/103/er(sentry)/ozone", "$(ProjectDir)"); + Project.SetDevice ("STM32F407IG"); + Project.SetHostIF ("USB", ""); + Project.SetTargetIF ("SWD"); + Project.SetTIFSpeed ("4 MHz"); + Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd"); + // + // User settings + // + File.Open ("D:/STM32CubeMX/103/Er(Sentry)/build/Debug/lll0121.elf"); +} + +/********************************************************************* +* +* OnStartupComplete +* +* Function description +* Called when program execution has reached/passed +* the startup completion point. Optional. +* +********************************************************************** +*/ +//void OnStartupComplete (void) { +//} + +/********************************************************************* +* +* TargetReset +* +* Function description +* Replaces the default target device reset routine. Optional. +* +* Notes +* This example demonstrates the usage when +* debugging an application in RAM on a Cortex-M target device. +* +********************************************************************** +*/ +//void TargetReset (void) { +// +// unsigned int SP; +// unsigned int PC; +// unsigned int VectorTableAddr; +// +// VectorTableAddr = Elf.GetBaseAddr(); +// // +// // Set up initial stack pointer +// // +// if (VectorTableAddr != 0xFFFFFFFF) { +// SP = Target.ReadU32(VectorTableAddr); +// Target.SetReg("SP", SP); +// } +// // +// // Set up entry point PC +// // +// PC = Elf.GetEntryPointPC(); +// +// if (PC != 0xFFFFFFFF) { +// Target.SetReg("PC", PC); +// } else if (VectorTableAddr != 0xFFFFFFFF) { +// PC = Target.ReadU32(VectorTableAddr + 4); +// Target.SetReg("PC", PC); +// } else { +// Util.Error("Project file error: failed to set entry point PC", 1); +// } +//} + +/********************************************************************* +* +* BeforeTargetReset +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetReset (void) { +//} + +/********************************************************************* +* +* AfterTargetReset +* +* Function description +* Event handler routine. Optional. +* The default implementation initializes SP and PC to reset values. +** +********************************************************************** +*/ +void AfterTargetReset (void) { + _SetupTarget(); +} + +/********************************************************************* +* +* DebugStart +* +* Function description +* Replaces the default debug session startup routine. Optional. +* +********************************************************************** +*/ +//void DebugStart (void) { +//} + +/********************************************************************* +* +* TargetConnect +* +* Function description +* Replaces the default target IF connection routine. Optional. +* +********************************************************************** +*/ +//void TargetConnect (void) { +//} + +/********************************************************************* +* +* BeforeTargetConnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetConnect (void) { +//} + +/********************************************************************* +* +* AfterTargetConnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetConnect (void) { +//} + +/********************************************************************* +* +* TargetDownload +* +* Function description +* Replaces the default program download routine. Optional. +* +********************************************************************** +*/ +//void TargetDownload (void) { +//} + +/********************************************************************* +* +* BeforeTargetDownload +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetDownload (void) { +//} + +/********************************************************************* +* +* AfterTargetDownload +* +* Function description +* Event handler routine. Optional. +* The default implementation initializes SP and PC to reset values. +* +********************************************************************** +*/ +void AfterTargetDownload (void) { + _SetupTarget(); +} + +/********************************************************************* +* +* BeforeTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetDisconnect (void) { +//} + +/********************************************************************* +* +* AfterTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetDisconnect (void) { +//} + +/********************************************************************* +* +* AfterTargetHalt +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetHalt (void) { +//} + +/********************************************************************* +* +* BeforeTargetResume +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetResume (void) { +//} + +/********************************************************************* +* +* OnSnapshotLoad +* +* Function description +* Called upon loading a snapshot. Optional. +* +* Additional information +* This function is used to restore the target state in cases +* where values cannot simply be written to the target. +* Typical use: GPIO clock needs to be enabled, before +* GPIO is configured. +* +********************************************************************** +*/ +//void OnSnapshotLoad (void) { +//} + +/********************************************************************* +* +* OnSnapshotSave +* +* Function description +* Called upon saving a snapshot. Optional. +* +* Additional information +* This function is usually used to save values of the target +* state which can either not be trivially read, +* or need to be restored in a specific way or order. +* Typically use: Memory Mapped Registers, +* such as PLL and GPIO configuration. +* +********************************************************************** +*/ +//void OnSnapshotSave (void) { +//} + +/********************************************************************* +* +* OnError +* +* Function description +* Called when an error ocurred. Optional. +* +********************************************************************** +*/ +//void OnError (void) { +//} + +/********************************************************************* +* +* AfterProjectLoad +* +* Function description +* After Project load routine. Optional. +* +********************************************************************** +*/ +//void AfterProjectLoad (void) { +//} + +/********************************************************************* +* +* OnDebugStartBreakSymbolReached +* +* Function description +* Called when program execution has reached/passed +* the symbol to be breaked at during debug start. Optional. +* +********************************************************************** +*/ +//void OnDebugStartBreakSymReached (void) { +//} + +/********************************************************************* +* +* _SetupTarget +* +* Function description +* Setup the target. +* Called by AfterTargetReset() and AfterTargetDownload(). +* +* Auto-generated function. May be overridden by Ozone. +* +********************************************************************** +*/ +void _SetupTarget(void) { + unsigned int SP; + unsigned int PC; + unsigned int VectorTableAddr; + + VectorTableAddr = Elf.GetBaseAddr(); + // + // Set up initial stack pointer + // + SP = Target.ReadU32(VectorTableAddr); + if (SP != 0xFFFFFFFF) { + Target.SetReg("SP", SP); + } + // + // Set up entry point PC + // + PC = Elf.GetEntryPointPC(); + if (PC != 0xFFFFFFFF) { + Target.SetReg("PC", PC); + } else { + Util.Error("Project script error: failed to set up entry point PC", 1); + } +} \ No newline at end of file diff --git a/Ozone/ozone.jdebug.user b/Ozone/ozone.jdebug.user new file mode 100644 index 0000000..5f3aa2e --- /dev/null +++ b/Ozone/ozone.jdebug.user @@ -0,0 +1,32 @@ + + +OpenDocument="atti_esti.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/atti_esti.c", Line=0 +OpenDocument="cmd.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/cmd.c", Line=0 +OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32CubeMX/103/Er(Sentry)/startup_stm32f407xx.s", Line=51 +OpenDocument="chassis.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/chassis.c", Line=0 +OpenDocument="ai.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/User/task/ai.c", Line=0 +OpenDocument="main.c", FilePath="D:/STM32CubeMX/103/Er(Sentry)/Core/Src/main.c", Line=62 +OpenToolbar="Debug", Floating=0, x=0, y=0 +OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=273, h=589, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=1, w=738, h=313, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=273, h=102, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=0, w=738, h=378, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=422, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1497, h=292, 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="1295;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1295;-10", CodeGraphLegendShown=1, CodeGraphLegendPosition="1254;5" +SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" +TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[191;100;100;100;931] +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=[100;100;100;100;100;100;100;107;107] +TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] +TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;105] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[210;144;100;263] +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="shoot_mcu_package", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="ai", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="mcu", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="chassis", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="cmd_chassis", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="rc_c", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="imu_eulr", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="ai_result", RefreshRate=5, Window=Watched Data 1 \ No newline at end of file diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c index 88c1ef4..b9a988a 100644 --- a/User/device/motor_rm.c +++ b/User/device/motor_rm.c @@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { switch (module) { case MOTOR_M2006: return 36.0f; - case MOTOR_M3508: return 3591.0f / 187.0f; + case MOTOR_M3508: return 268.0f / 17.0f; case MOTOR_GM6020: return 1.0f; default: return 1.0f; } diff --git a/User/module/chassis.c b/User/module/chassis.c index 012a5af..b500c7f 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -81,6 +81,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now) PID_Reset(&c->pid.chassis_3508VPID[i]); PID_Reset(&c->pid.chassis_6020OmegaPid[i]); PID_Reset(&c->pid.chassis_6020anglePid[i]); + PID_Reset(&c->pid.chassis_3508DAOHANG_pid[i]); } c->mode = mode; @@ -142,6 +143,10 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq) PID_Init(&c->pid.chassis_6020OmegaPid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Omega_param); PID_Init(&c->pid.chassis_6020anglePid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Angle_param); } + for (int i = 0; i < 4; i++) + { + PID_Init(&c->pid.chassis_3508DAOHANG_pid[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508DAOHANG_param); + } PID_Init(&c->pid.chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->chassis_follow_gimbalPID); LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx @@ -320,7 +325,7 @@ int8_t Chassis_update(Chassis_t *c) c->motorfeedback.rotor_rpm6020[i] = (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / 320); c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) /10000; // c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)); - + c->motorfeedback.rotor_truespeed3508[i]=MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor))*17.0f/268.0f/60.0f*M_PI*0.116f; c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0); if (c->motorfeedback.rotor_angle6020[i] < 0) { @@ -404,9 +409,9 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now) case CHASSIS_MODE_DAOHANG: // 导航模式 - c->move_vec.Vx = -c_cmd->Vx/10; - c->move_vec.Vy = c_cmd->Vy/10; - c->move_vec.Vw = c_cmd->Vw/10; + c->move_vec.Vx = -c_cmd->Vx/1; + c->move_vec.Vy = c_cmd->Vy/1; + c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);; break; default: @@ -420,6 +425,14 @@ case CHASSIS_MODE_DAOHANG: Chassis_speed_calculate(c, c_cmd); + switch (c->mode) + { case CHASSIS_MODE_BREAK: + case CHASSIS_MODE_RELAX: + case STOP: + case RC: + case CHASSIS_MODE_FOLLOW_GIMBAL: + case CHASSIS_MODE_ROTOR: + for (int i = 0; i < 4; i++) { float chassis6020_detangle[4]; // 6020解算出的角度 @@ -437,6 +450,26 @@ case CHASSIS_MODE_DAOHANG: c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt); c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]); } + + case CHASSIS_MODE_DAOHANG: +for(int i=0;i<4;i++) +{ + float chassis6020_detangle[4]; // 6020解算出的角度 + c->hopemotorout.motor6020_target[i] = c->hopemotorout.rotor6020_jiesuan_2[i]; + chassis6020_detangle[i] = PID_Calc(&(c->pid.chassis_6020anglePid[i]), c->hopemotorout.motor6020_target[i], + c->motorfeedback.rotor_angle6020[i], 0.0f, c->dt); + // c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个 + c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i], + c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt); + + c->out.rotor6020_out[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_6020out[i]); + + c->hopemotorout.motor3508_target[i] = c->hopemotorout.rotor3508_jiesuan_2[i]; + c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508DAOHANG_pid[i]), c->hopemotorout.motor3508_target[i], + c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt); + c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]); +} +} return CHASSIS_OK; } diff --git a/User/module/chassis.h b/User/module/chassis.h index c115718..e46a923 100644 --- a/User/module/chassis.h +++ b/User/module/chassis.h @@ -55,6 +55,9 @@ extern "C" KPID_Params_t C6020Omega_param; KPID_Params_t C6020Angle_param; KPID_Params_t M3508v_param; + + KPID_Params_t M3508DAOHANG_param; + KPID_Params_t chassis_follow_gimbalPID; MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机 @@ -113,6 +116,7 @@ extern "C" { fp32 rotor_rpm3508[4]; fp32 rotor_current3508[4]; + fp32 rotor_truespeed3508[4]; fp32 rotor_rpm6020[4]; fp32 rotor_angle6020[4]; fp32 rotor_current6020[4]; @@ -126,6 +130,9 @@ extern "C" KPID_t chassis_6020anglePid[4]; KPID_t chassis_6020OmegaPid[4]; KPID_t chassis_3508VPID[4]; + + KPID_t chassis_3508DAOHANG_pid[4]; + KPID_t chassis_follow_gimbal_pid; } pid; diff --git a/User/module/config.c b/User/module/config.c index 0d728d6..ed2e45c 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -65,6 +65,18 @@ static const Config_Param_t config = { .d_cutoff_freq = -1.0f, .range = -1.0f }, + + .M3508DAOHANG_param={ + .k = 1.0f, + .p = 2.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = -1.0f + }, + .chassis_follow_gimbalPID = { .k = 0.7f, .p = 0.5f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 32db2f1..b625241 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -246,7 +246,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ Gimbal_SetMode(g, g_cmd->mode); /*处理小yaw轴控制命令*/ -float delta_yaw_6020 = 20.0f*g_cmd->delta_yaw_6020 * g->dt; +float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt; if(g->param->travel.yaw_6020 > 0) // 有限位才处理 {