robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: robofish:476268298a665b4d60ed95466847d4631fb8af53
Branches Tags
robofish:main
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
...
compare: robofish:f641fe4051597e9eedbe113f34bdae0a1f8e8384
Branches Tags
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
robofish:main

6 Commits
476268298a ... f641fe4051

Author SHA1 Message Date
Robofish
f641fe4051 更新shoot 2025-10-02 01:19:13 +08:00
Robofish
d0727de06f 改完bug 2025-10-01 18:50:38 +08:00
Robofish
fbeb29513d 修云台 2025-10-01 17:38:21 +08:00
Robofish
65571643d6 修改rc_can 2025-10-01 15:07:01 +08:00
Robofish
e1c1c07b52 修rc发送 2025-10-01 03:20:52 +08:00
Robofish
fcbb76fda4 修改遥控 2025-10-01 03:03:41 +08:00
18 changed files with 1876 additions and 1653 deletions
Show Stats Expand all files Collapse all files

12
.mxproject
View File

File diff suppressed because one or more lines are too long

2
Core/Inc/stm32f4xx_it.h
View File

@ -59,6 +59,7 @@ void EXTI3_IRQHandler(void);
void EXTI4_IRQHandler(void);
void DMA1_Stream1_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void);
void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void);
void EXTI9_5_IRQHandler(void);
@ -69,6 +70,7 @@ void TIM7_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void);
void DMA2_Stream2_IRQHandler(void);
void DMA2_Stream3_IRQHandler(void);
void CAN2_TX_IRQHandler(void);
void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void);
void OTG_FS_IRQHandler(void);

6
Core/Src/can.c
View File

@ -122,6 +122,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* CAN1 interrupt Init */
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
@ -155,6 +157,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* CAN2 interrupt Init */
HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_TX_IRQn);
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
@ -186,6 +190,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);
/* CAN1 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN1_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
/* USER CODE BEGIN CAN1_MspDeInit 1 */
@ -211,6 +216,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
/* CAN2 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
/* USER CODE BEGIN CAN2_MspDeInit 1 */

28
Core/Src/stm32f4xx_it.c
View File

@ -269,6 +269,20 @@ void DMA1_Stream2_IRQHandler(void)
/* USER CODE END DMA1_Stream2_IRQn 1 */
}
/**
* @brief This function handles CAN1 TX interrupts.
*/
void CAN1_TX_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_TX_IRQn 0 */
/* USER CODE END CAN1_TX_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_TX_IRQn 1 */
/* USER CODE END CAN1_TX_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX0 interrupts.
*/
@ -410,6 +424,20 @@ void DMA2_Stream3_IRQHandler(void)
/* USER CODE END DMA2_Stream3_IRQn 1 */
}
/**
* @brief This function handles CAN2 TX interrupts.
*/
void CAN2_TX_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_TX_IRQn 0 */
/* USER CODE END CAN2_TX_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_TX_IRQn 1 */
/* USER CODE END CAN2_TX_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX0 interrupts.
*/

4
DevC.ioc
View File

@ -267,8 +267,10 @@ MxDb.Version=DB.6.0.20
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream7_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
@ -552,7 +554,7 @@ ProjectManager.ProjectName=DevC
ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x1000
ProjectManager.TargetToolchain=MDK-ARM V5.32
ProjectManager.TargetToolchain=CMake
ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath=

4
Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h
View File

@ -1245,8 +1245,8 @@ typedef struct
*/
/* Initialization and de-initialization functions ******************************/
HAL_StatusTypeDef HAL_RCC_DeInit(void);
HAL_StatusTypeDef HAL_RCC_OscConfig(const RCC_OscInitTypeDef *RCC_OscInitStruct);
HAL_StatusTypeDef HAL_RCC_ClockConfig(const RCC_ClkInitTypeDef *RCC_ClkInitStruct, uint32_t FLatency);
HAL_StatusTypeDef HAL_RCC_OscConfig(RCC_OscInitTypeDef *RCC_OscInitStruct);
HAL_StatusTypeDef HAL_RCC_ClockConfig(RCC_ClkInitTypeDef *RCC_ClkInitStruct, uint32_t FLatency);
/**
* @}
*/

4
Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c
View File

@ -218,7 +218,7 @@ __weak HAL_StatusTypeDef HAL_RCC_DeInit(void)
* first and then HSE On or HSE Bypass.
* @retval HAL status
*/
__weak HAL_StatusTypeDef HAL_RCC_OscConfig(const RCC_OscInitTypeDef *RCC_OscInitStruct)
__weak HAL_StatusTypeDef HAL_RCC_OscConfig(RCC_OscInitTypeDef *RCC_OscInitStruct)
{
uint32_t tickstart, pll_config;
@ -590,7 +590,7 @@ __weak HAL_StatusTypeDef HAL_RCC_OscConfig(const RCC_OscInitTypeDef *RCC_OscIni
* (for more details refer to section above "Initialization/de-initialization functions")
* @retval None
*/
HAL_StatusTypeDef HAL_RCC_ClockConfig(const RCC_ClkInitTypeDef *RCC_ClkInitStruct, uint32_t FLatency)
HAL_StatusTypeDef HAL_RCC_ClockConfig(RCC_ClkInitTypeDef *RCC_ClkInitStruct, uint32_t FLatency)
{
uint32_t tickstart;

2838
MDK-ARM/DevC.uvoptx
View File

File diff suppressed because it is too large Load Diff

115
MDK-ARM/DevC.uvprojx
View File

@ -1,10 +1,7 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
<?xml version="1.0" encoding="UTF-8"?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" noNamespaceSchemaLocation="project_projx.xsd">
<SchemaVersion>2.1</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Targets>
<Target>
<TargetName>DevC</TargetName>
@ -20,28 +17,28 @@
<PackID>Keil.STM32F4xx_DFP.3.0.0</PackID>
<PackURL>https://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ</Cpu>
<FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile>
<FlashDriverDll></FlashDriverDll>
<FlashUtilSpec />
<StartupFile />
<FlashDriverDll />
<DeviceId>0</DeviceId>
<RegisterFile></RegisterFile>
<MemoryEnv></MemoryEnv>
<Cmp></Cmp>
<Asm></Asm>
<Linker></Linker>
<OHString></OHString>
<InfinionOptionDll></InfinionOptionDll>
<SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc>
<RegisterFile />
<MemoryEnv />
<Cmp />
<Asm />
<Linker />
<OHString />
<InfinionOptionDll />
<SLE66CMisc />
<SLE66AMisc />
<SLE66LinkerMisc />
<SFDFile>$$Device:STM32F407IGHx$CMSIS\SVD\STM32F407.svd</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv>
<BinPath></BinPath>
<IncludePath></IncludePath>
<LibPath></LibPath>
<RegisterFilePath></RegisterFilePath>
<DBRegisterFilePath></DBRegisterFilePath>
<BinPath />
<IncludePath />
<LibPath />
<RegisterFilePath />
<DBRegisterFilePath />
<TargetStatus>
<Error>0</Error>
<ExitCodeStop>0</ExitCodeStop>
@ -56,15 +53,15 @@
<CreateHexFile>1</CreateHexFile>
<DebugInformation>1</DebugInformation>
<BrowseInformation>1</BrowseInformation>
<ListingPath></ListingPath>
<ListingPath />
<HexFormatSelection>1</HexFormatSelection>
<Merge32K>0</Merge32K>
<CreateBatchFile>0</CreateBatchFile>
<BeforeCompile>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Name />
<UserProg2Name />
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopU1X>0</nStopU1X>
@ -73,8 +70,8 @@
<BeforeMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Name />
<UserProg2Name />
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopB1X>0</nStopB1X>
@ -83,15 +80,15 @@
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>1</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Name />
<UserProg2Name />
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopA1X>0</nStopA1X>
<nStopA2X>0</nStopA2X>
</AfterMake>
<SelectedForBatchBuild>1</SelectedForBatchBuild>
<SVCSIdString></SVCSIdString>
<SVCSIdString />
</TargetCommonOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
@ -105,8 +102,8 @@
<AssembleAssemblyFile>0</AssembleAssemblyFile>
<PublicsOnly>0</PublicsOnly>
<StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<CustomArgument />
<IncludeLibraryModules />
<ComprImg>0</ComprImg>
</CommonProperty>
<DllOption>
@ -140,10 +137,10 @@
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3>
<Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<Flash4 />
<pFcarmOut />
<pFcarmGrp />
<pFcArmRoot />
<FcArmLst>0</FcArmLst>
</Utilities>
<TargetArmAds>
@ -176,7 +173,7 @@
<RvctClst>0</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M4"</AdsCpuType>
<RvctDeviceName></RvctDeviceName>
<RvctDeviceName />
<mOS>0</mOS>
<uocRom>0</uocRom>
<uocRam>0</uocRam>
@ -310,7 +307,7 @@
<Size>0x4000</Size>
</OCR_RVCT10>
</OnChipMemories>
<RvctStartVector></RvctStartVector>
<RvctStartVector />
</ArmAdsMisc>
<Cads>
<interw>1</interw>
@ -337,9 +334,9 @@
<v6WtE>0</v6WtE>
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<MiscControls />
<Define>USE_HAL_DRIVER,STM32F407xx</Define>
<Undefine></Undefine>
<Undefine />
<IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../User</IncludePath>
</VariousControls>
</Cads>
@ -355,9 +352,9 @@
<useXO>0</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<MiscControls />
<Define />
<Undefine />
<IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include</IncludePath>
</VariousControls>
</Aads>
@ -368,15 +365,15 @@
<noStLib>0</noStLib>
<RepFail>1</RepFail>
<useFile>0</useFile>
<TextAddressRange></TextAddressRange>
<DataAddressRange></DataAddressRange>
<pXoBase></pXoBase>
<ScatterFile></ScatterFile>
<IncludeLibs></IncludeLibs>
<IncludeLibsPath></IncludeLibsPath>
<Misc></Misc>
<LinkerInputFile></LinkerInputFile>
<DisabledWarnings></DisabledWarnings>
<TextAddressRange />
<DataAddressRange />
<pXoBase />
<ScatterFile />
<IncludeLibs />
<IncludeLibsPath />
<Misc />
<LinkerInputFile />
<DisabledWarnings />
</LDads>
</TargetArmAds>
</TargetOption>
@ -922,20 +919,18 @@
</Groups>
</Target>
</Targets>
<RTE>
<apis/>
<apis />
<components>
<component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="6.1.0" condition="ARMv6_7_8-M Device">
<package name="CMSIS" schemaVersion="1.7.36" url="https://www.keil.com/pack/" vendor="ARM" version="6.1.0"/>
<package name="CMSIS" schemaVersion="1.7.36" url="https://www.keil.com/pack/" vendor="ARM" version="6.1.0" />
<targetInfos>
<targetInfo name="DevC"/>
<targetInfo name="DevC" />
</targetInfos>
</component>
</components>
<files/>
<files />
</RTE>
<LayerInfo>
<Layers>
<Layer>
@ -944,5 +939,5 @@
</Layer>
</Layers>
</LayerInfo>
</Project>

29
User/device/motor_dm.h
View File

@ -43,7 +43,7 @@ typedef struct {
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @brief 注册一个DM电机
* @param param 电机参数
* @return
*/
@ -62,11 +62,31 @@ int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
*/
int8_t MOTOR_DM_UpdateAll(void);
/**
* @brief MIT模式控制电机
* @param param 电机参数
* @param output MIT控制输出参数
* @return 操作结果，0为成功，负值为失败
*/
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
/**
* @brief 位置速度双环控制电机
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 操作结果，0为成功，负值为失败
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
/**
* @brief 位置速度双环控制电机
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 操作结果，0为成功，负值为失败
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
/**
* @brief 获取指定电机的实例指针
@ -75,6 +95,11 @@ int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
/**
* @brief 使能电机
* @param param 电机参数
* @return 操作结果，0为成功，负值为失败
*/
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
/**

12
User/device/motor_rm.c
View File

@ -139,13 +139,13 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI;
}
while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI;
}
if (motor->motor.reverse) {
while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI;
}
while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI;
}
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
motor->feedback.torque_current = -motor->feedback.torque_current;

66
User/device/rc_can.c
View File

@ -93,46 +93,60 @@ int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
}
BSP_CAN_StdDataFrame_t frame;
frame.dlc = 8;
// 边界裁剪宏
#define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
switch (data_type) {
case RC_CAN_DATA_JOYSTICK:
case RC_CAN_DATA_JOYSTICK: {
frame.id = rc_can->param.joy_id;
frame.data[0] = (uint8_t)((int16_t)(rc_can->data.joy.ch_l_x * 32768.0f) & 0xFF);
frame.data[1] =
(uint8_t)(((int16_t)(rc_can->data.joy.ch_l_x * 32768.0f) >> 8) & 0xFF);
frame.data[2] = (uint8_t)((int16_t)(rc_can->data.joy.ch_l_y * 32768.0f) & 0xFF);
frame.data[3] =
(uint8_t)(((int16_t)(rc_can->data.joy.ch_l_y * 32768.0f) >> 8) & 0xFF);
frame.data[4] = (uint8_t)((int16_t)(rc_can->data.joy.ch_r_x * 32768.0f) & 0xFF);
frame.data[5] =
(uint8_t)(((int16_t)(rc_can->data.joy.ch_r_x * 32768.0f) >> 8) & 0xFF);
frame.data[6] = (uint8_t)((int16_t)(rc_can->data.joy.ch_r_y * 32768.0f) & 0xFF);
frame.data[7] =
(uint8_t)(((int16_t)(rc_can->data.joy.ch_r_y * 32768.0f) >> 8) & 0xFF);
float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
int16_t l_x_i = (int16_t)(l_x * 32768.0f);
int16_t l_y_i = (int16_t)(l_y * 32768.0f);
int16_t r_x_i = (int16_t)(r_x * 32768.0f);
int16_t r_y_i = (int16_t)(r_y * 32768.0f);
frame.data[0] = (uint8_t)(l_x_i & 0xFF);
frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
frame.data[2] = (uint8_t)(l_y_i & 0xFF);
frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
frame.data[4] = (uint8_t)(r_x_i & 0xFF);
frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
frame.data[6] = (uint8_t)(r_y_i & 0xFF);
frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
break;
case RC_CAN_DATA_SWITCH:
}
case RC_CAN_DATA_SWITCH: {
frame.id = rc_can->param.sw_id;
frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
frame.data[2] = (uint8_t)((int16_t)(rc_can->data.sw.ch_res * 32768.0f) & 0xFF);
frame.data[3] =
(uint8_t)(((int16_t)(rc_can->data.sw.ch_res * 32768.0f) >> 8) & 0xFF);
float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
frame.data[4] = 0; // 保留字节
frame.data[5] = 0; // 保留字节
frame.data[6] = 0; // 保留字节
frame.data[7] = 0; // 保留字节
break;
case RC_CAN_DATA_MOUSE:
}
case RC_CAN_DATA_MOUSE: {
frame.id = rc_can->param.mouse_id;
frame.data[0] = (uint8_t)((int16_t)(rc_can->data.mouse.x) & 0xFF);
frame.data[1] = (uint8_t)(((int16_t)(rc_can->data.mouse.x) >> 8) & 0xFF);
frame.data[2] = (uint8_t)((int16_t)(rc_can->data.mouse.y) & 0xFF);
frame.data[3] = (uint8_t)(((int16_t)(rc_can->data.mouse.y) >> 8) & 0xFF);
frame.data[4] = (uint8_t)((int16_t)(rc_can->data.mouse.z) & 0xFF);
frame.data[5] = (uint8_t)(((int16_t)(rc_can->data.mouse.z) >> 8) & 0xFF);
// 鼠标x/y/z一般为增量，若有极限也可加clamp
int16_t x = (int16_t)(rc_can->data.mouse.x);
int16_t y = (int16_t)(rc_can->data.mouse.y);
int16_t z = (int16_t)(rc_can->data.mouse.z);
frame.data[0] = (uint8_t)(x & 0xFF);
frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
frame.data[2] = (uint8_t)(y & 0xFF);
frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
frame.data[4] = (uint8_t)(z & 0xFF);
frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
break;
case RC_CAN_DATA_KEYBOARD:
}
case RC_CAN_DATA_KEYBOARD: {
frame.id = rc_can->param.keyboard_id;
frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
@ -140,9 +154,11 @@ int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
}
break;
}
default:
return DEVICE_ERR;
}
#undef RC_CAN_CLAMP
if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
frame.data, frame.dlc) != BSP_OK) {
return DEVICE_ERR;

37
User/module/config.c
View File

@ -23,6 +23,8 @@
Config_RobotParam_t robot_config = {
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=0.05f,
.shot_burst_num=1,
.fric_motor_param[0] = {
.can = BSP_CAN_2,
.id = 0x201,
@ -41,7 +43,7 @@ Config_RobotParam_t robot_config = {
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M2006,
.reverse = false,
.reverse = true,
.gear=true,
},
.fric_follow = {
@ -54,6 +56,7 @@ Config_RobotParam_t robot_config = {
.d_cutoff_freq=30.0f,
.range=-1.0f,
},
.fric_err = {
.k=1.0f,
.p=4.0f,
@ -64,15 +67,25 @@ Config_RobotParam_t robot_config = {
.d_cutoff_freq=40.0f,
.range=-1.0f,
},
.trig_omg = {
.k=1.0f,
.p=1.5f,
.i=0.3f,
.d=0.5f,
.i_limit=0.2f,
.out_limit=0.9f,
.d_cutoff_freq=-1.0f,
.range=-1.0f,
},
.trig = {
.k=1.0f,
.p=1.2f,
.i=0.0f,
.p=1.0f,
.i=0.1f,
.d=0.05f,
.i_limit=0.2f,
.out_limit=0.9f,
.d_cutoff_freq=30.0f,
.range=-1.0f,
.i_limit=0.8f,
.out_limit=0.5f,
.d_cutoff_freq=-1.0f,
.range=M_2PI,
},
.filter.fric = {
.in = 30.0f,
@ -82,13 +95,13 @@ Config_RobotParam_t robot_config = {
.in = 30.0f,
.out = 30.0f,
},
},
.gimbal_param = {
.pid = {
.yaw_omega = {
.k = 0.5f,
.k = 1.0f,
.p = 1.0f,
.i = 0.5f,
.d = 0.0f,
@ -101,7 +114,7 @@ Config_RobotParam_t robot_config = {
.k = 10.0f,
.p = 3.0f,
.i = 0.0f,
.d = 0.1f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
@ -130,7 +143,7 @@ Config_RobotParam_t robot_config = {
},
.mech_zero = {
.yaw = 0.0f,
.pit = 3.8f,
.pit = 1.77f,
},
.travel = {
.yaw = -1.0f,
@ -142,7 +155,7 @@ Config_RobotParam_t robot_config = {
},
.pit_motor ={
.can = BSP_CAN_2,
.id = 0x209,
.id = 0x206,
.gear = false,
.module = MOTOR_GM6020,
.reverse = true,

15
User/module/gimbal.c
View File

@ -6,6 +6,7 @@
#include "gimbal.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include <math.h>
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor_dm.h"
@ -158,7 +159,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
Gimbal_SetMode(g, g_cmd->mode);
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
float delta_yaw = g_cmd->delta_yaw * g->dt;
float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
if (g->param->travel.yaw > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
@ -217,15 +218,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt);
// yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
// g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
// g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
// g->feedback.imu.gyro.z, 0.f, g->dt);
// pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
// g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
// g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
// g->feedback.imu.gyro.y, 0.f, g->dt);
break;
/* 输出滤波 */
@ -245,7 +239,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
void Gimbal_Output(Gimbal_t *g){
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
MOTOR_MIT_Output_t output = {0};
output.torque = g->out.yaw * 2.5;
output.torque = g->out.yaw * 6.0f; // 乘以减速比
output.kd = 0.5f;
MOTOR_RM_Ctrl(&g->param->pit_motor);
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
}

306
User/module/shoot.c
View File

@ -1,16 +1,13 @@
#include "shoot.h"
#include "bsp/can.h"
#include "component/filter.h"
#include <string.h>
// #include
#include "can.h"
#include "component/filter.h"
#include "component/user_math.h"
#include <math.h>
#include "bsp/time.h"
uint32_t shoot_ctrl_cnt_last;
float shoot_ctrl_dt;
bool last_firecmd=false;
static bool last_firecmd;
static inline void ScaleSumTo1(float *a, float *b) {
float sum = *a + *b;
@ -21,148 +18,281 @@ static inline void ScaleSumTo1(float *a, float *b) {
}
}
int8_t Shoot_Init(Shoot_t *c, Shoot_Params_t *param, float target_freq)
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
{
if (c == NULL || param == NULL || target_freq <= 0.0f) {
if (s == NULL) {
return -1; // 参数错误
}
c->param=param;
BSP_CAN_Init();
memset(&c->feedback, 0, sizeof(c->feedback));
s->mode=mode;
return 0;
}
int8_t Shoot_ResetIntegral(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_ResetIntegral(&s->pid.fric_follow[i]);
PID_ResetIntegral(&s->pid.fric_err[i]);
}
PID_ResetIntegral(&s->pid.trig);
PID_ResetIntegral(&s->pid.trig_omg);
return 0;
}
int8_t Shoot_ResetCalu(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_Reset(&s->pid.fric_follow[i]);
PID_Reset(&s->pid.fric_err[i]);
LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
}
PID_Reset(&s->pid.trig);
PID_Reset(&s->pid.trig_omg);
LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
return 0;
}
int8_t Shoot_ResetOutput(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
s->output.out_follow[i]=0.0f;
s->output.out_err[i]=0.0f;
s->output.out_fric[i]=0.0f;
s->output.lpfout_fric[i]=0.0f;
}
s->output.outagl_trig=0.0f;
s->output.outomg_trig=0.0f;
s->output.outlpf_trig=0.0f;
return 0;
}
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
{
if (s == NULL) {
return -1; // 参数错误
}
s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
return 0;
}
/**
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
*
* \param s 包含发射数据的结构体
* \param num 需要发射的弹丸数量
*/
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL || s->shoot_Anglecalu.num_to_shoot == 0) {
return -1;
}
if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
{
s->shoot_Anglecalu.time_last_shoot=s->now;
s->target_variable.target_angle += s->param->trig_step_angle;
if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI;
else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI;
s->shoot_Anglecalu.num_to_shoot--;
}
return 0;
}
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
{
if (s == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误
}
s->param=param;
BSP_CAN_Init();
for(int i=0;i<SHOOT_FRIC_NUM;i++){
MOTOR_RM_Register(&param->fric_motor_param[i]);
PID_Init(&c->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
LowPassFilter2p_Init(&c->filter.fric.in[i], target_freq, c->param->filter.fric.in);
LowPassFilter2p_Init(&c->filter.fric.out[i], target_freq, c->param->filter.fric.out);
PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
}
MOTOR_RM_Register(&param->trig_motor_param);
PID_Init(&c->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
LowPassFilter2p_Init(&c->filter.trig.in, target_freq, c->param->filter.trig.in);
LowPassFilter2p_Init(&c->filter.trig.out, target_freq, c->param->filter.trig.out);
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);
LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=4000.0f;
return 0;
}
int8_t Chassis_UpdateFeedback(Shoot_t *c)
int8_t Chassis_UpdateFeedback(Shoot_t *s)
{
if (c == NULL) {
if (s == NULL) {
return -1; // 参数错误
}
float rpm_sum=0.0f;
for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
MOTOR_RM_Update(&c->param->fric_motor_param[i]);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&c->param->fric_motor_param[i]);
/* 更新摩擦电机反馈 */
MOTOR_RM_Update(&s->param->fric_motor_param[i]);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
if(motor_fed!=NULL)
{
c->feedback.fric[i]=motor_fed->motor.feedback;
s->feedback.fric[i]=motor_fed->motor.feedback;
}
c->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&c->filter.fric.in[i], c->feedback.fric[i].rotor_speed);
c->feedback.fric_rpm[i] = c->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
if(c->feedback.fric_rpm[i]>1.0f)c->feedback.fric_rpm[i]=1.0f;
if(c->feedback.fric_rpm[i]<-1.0f)c->feedback.fric_rpm[i]=-1.0f;
rpm_sum+=c->feedback.fric_rpm[i];
/* 滤波反馈rpm */
s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
/* 归一化rpm */
s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f;
if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f;
/* 计算平均rpm */
rpm_sum+=s->feedback.fric_rpm[i];
}
c->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
MOTOR_RM_Update(&c->param->trig_motor_param);
c->feedback.trig = MOTOR_RM_GetMotor(&c->param->trig_motor_param);
//
c->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&c->filter.trig.in, c->feedback.trig->feedback.rotor_speed);
c->feedback.trig_rpm = c->feedback.trig->feedback.rotor_speed / MAX_TRIG_RPM;
if(c->feedback.trig_rpm>1.0f)c->feedback.trig_rpm=1.0f; //如果单环效果好就删
if(c->feedback.trig_rpm<-1.0f)c->feedback.trig_rpm=-1.0f;
//
c->errtosee = c->feedback.fric[0].rotor_speed - c->feedback.fric[1].rotor_speed;
s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
/* 更新拨弹电机反馈 */
MOTOR_RM_Update(&s->param->trig_motor_param);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
if(motor_fed!=NULL)
{
s->feedback.trig=motor_fed->feedback;
}
/* 将多圈角度归化到单圈 (-M_PI, M_PI) */
s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π]
if (s->feedback.trig_angle_cicle > M_PI) {
s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π]
}else if (s->feedback.trig_angle_cicle < -M_PI) {
s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
}
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM;
if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f; //如果单环效果好就删
if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
return 0;
}
int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd)
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (c == NULL || cmd == NULL) {
if (s == NULL || cmd == NULL) {
return -1; // 参数错误
}
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
c->lask_wakeup = BSP_TIME_Get_us();
c->online = cmd->online;
if(!c->online /*|| c->mode==SHOOT_MODE_SAFE*/){
s->now = BSP_TIME_Get_us() / 1000000.0f;
s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
s->lask_wakeup = BSP_TIME_Get_us();
s->online = cmd->online;
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
MOTOR_RM_Relax(&c->param->fric_motor_param[i]);
MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
}
MOTOR_RM_Relax(&c->param->trig_motor_param);
MOTOR_RM_Relax(&s->param->trig_motor_param);
}
else{
switch(c->running_state)
switch(s->running_state)
{
case SHOOT_STATE_IDLE:/*熄火等待*/
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_ResetIntegral(&c->pid.fric_follow[i]);
c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],0.0f,c->feedback.fric_rpm[i],0,c->dt);
c->output.out_fric[i]=c->output.out_follow[i];
c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
PID_ResetIntegral(&s->pid.fric_follow[i]);
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_fric[i]=s->output.out_follow[i];
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
}
c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(cmd->ready)
{
c->running_state=SHOOT_STATE_READY;
{
Shoot_ResetCalu(s);
Shoot_ResetIntegral(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_READY;
}
break;
case SHOOT_STATE_READY:/*准备射击*/
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{ /* 计算跟随输出->计算修正输出->加和、滤波、输出 */
// c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);
c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i];
c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
{ /* 计算跟随输出、计算修正输出 */
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
/* 按比例缩放并加和输出 */
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
/* 滤波 */
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
/* 输出 */
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
}
c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
/* 拨弹电机输出 */
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
/* 检查状态机 */
if(!cmd->ready)
{
c->running_state=SHOOT_STATE_IDLE;
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_IDLE;
}
else if(cmd->last_firecmd==false&&cmd->firecmd==true)//可以加一个到达目标速度的判断
else if(last_firecmd==false&&cmd->firecmd==true)
{
c->running_state=SHOOT_STATE_FIRE;
c->target_variable.target_angle-=c->param->trig_step_angle;
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_FIRE;
s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
}
break;
case SHOOT_STATE_FIRE:
// c->target_variable.target_angle+=c->param->trig_step_angle;
Shoot_CaluTargetAngle(s, cmd);
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
// c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);
c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i];
c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
}
c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(!cmd->firecmd)
{
c->running_state=SHOOT_STATE_READY;
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_READY;
}
break;
default:
c->running_state=SHOOT_STATE_IDLE;
s->running_state=SHOOT_STATE_IDLE;
break;
}
}
MOTOR_RM_Ctrl(&c->param->fric_motor_param[0]);
MOTOR_RM_Ctrl(&c->param->trig_motor_param);
MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
MOTOR_RM_Ctrl(&s->param->trig_motor_param);
last_firecmd = cmd->firecmd;
return 0;
}

43
User/module/shoot.h
View File

@ -23,7 +23,7 @@ extern "C" {
#define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
#define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 1000.0f
#define MAX_TRIG_RPM 5000.0f
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
@ -45,23 +45,28 @@ typedef struct {
bool ready; /* 准备射击 */
bool firecmd; /* 射击指令 */
bool last_firecmd;
} Shoot_CMD_t;
typedef struct {
MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
MOTOR_RM_t *trig; /* 拨弹电机反馈 */
MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */
float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
float fric_avgrpm; /* 归一化摩擦轮平均转速*/
float trig_rpm; /* 归一化拨弹电机转速*/
}Shoot_Feedback_t;
typedef struct{
float time_last_shoot;
uint8_t num_to_shoot;
uint8_t num_shooted;
}Shoot_AngleCalu_t;
typedef struct {
float out_follow[SHOOT_FRIC_NUM];
float out_err[SHOOT_FRIC_NUM];
@ -69,14 +74,17 @@ typedef struct {
float lpfout_fric[SHOOT_FRIC_NUM];
float out_trig;
float lpfout_trig;
float outagl_trig;
float outomg_trig;
float outlpf_trig;
}Shoot_Output_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
float trig_step_angle; /* 每次拨弹电机转动的角度 */
float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
float shot_delay_time; /* 射击间隔时间，单位秒 */
uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM];
MOTOR_RM_Param_t trig_motor_param;
@ -85,7 +93,7 @@ typedef struct {
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
KPID_Params_t trig; /* 拨弹电机PID控制参数 */
KPID_Params_t trig_omg; /* 拨弹电机PID控制参数 */
/* 低通滤波器截止频率 */
struct {
@ -107,8 +115,9 @@ typedef struct {
typedef struct {
bool online;
float now;
uint64_t lask_wakeup;
float dt;
float dt;
Shoot_Params_t *param; /* */
/* 模块通用 */
@ -117,6 +126,7 @@ typedef struct {
/* 反馈信息 */
Shoot_Feedback_t feedback;
/* 控制信息*/
Shoot_AngleCalu_t shoot_Anglecalu;
Shoot_Output_t output;
/* 目标控制量 */
struct {
@ -129,6 +139,7 @@ typedef struct {
KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
KPID_t fric_err[SHOOT_FRIC_NUM]; /* */
KPID_t trig;
KPID_t trig_omg;
} pid;
/* 滤波器 */
@ -152,32 +163,32 @@ typedef struct {
/**
* \brief 初始化发射
*
* \param c 包含发射数据的结构体
* \param s 包含发射数据的结构体
* \param param 包含发射参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Shoot_Init(Shoot_t *c, Shoot_Params_t *param, float target_freq);
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
/**
* \brief 更新反馈
*
* \param c 包含发射数据的结构体
* \param s 包含发射数据的结构体
*
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Shoot_t *c);
int8_t Chassis_UpdateFeedback(Shoot_t *s);
/**
* \brief 初始化发射
*
* \param c 包含发射数据的结构体
* \param s 包含发射数据的结构体
* \param cmd 包含发射命令的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd);
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);

4
User/task/rc.c
View File

@ -73,7 +73,7 @@ void Task_rc(void *argument) {
rc_can.data.joy.ch_r_y = dr16.data.ch_r_y;
rc_can.data.sw.sw_l = (RC_CAN_SW_t)dr16.data.sw_l;
rc_can.data.sw.sw_r = (RC_CAN_SW_t)dr16.data.sw_r;
rc_can.data.sw.ch_res = dr16.data.res;
rc_can.data.sw.ch_res = dr16.data.ch_res;
RC_CAN_SendData(&rc_can, RC_CAN_DATA_JOYSTICK);
RC_CAN_SendData(&rc_can, RC_CAN_DATA_SWITCH);
@ -90,7 +90,7 @@ void Task_rc(void *argument) {
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
break;
case DR16_SW_DOWN:
cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x* 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
break;

4
debug.jdebug
View File

@ -22,12 +22,12 @@ void OnProjectLoad (void) {
//
// Dialog-generated settings
//
Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)");
Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)");
Project.SetDevice ("STM32F407IG");
Project.SetHostIF ("USB", "207400620");
Project.SetTargetIF ("SWD");
Project.SetTIFSpeed ("4 MHz");
Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)");
Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)");
Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
//
// User settings