mirror of
https://github.com/goldenfishs/MRobot.git
synced 2026-02-04 18:00:19 +08:00
Compare commits
5 Commits
22ea6e14b3
...
7d8ae97c25
| Author | SHA1 | Date | |
|---|---|---|---|
| 7d8ae97c25 | |||
| 296118b408 | |||
| 7300d03812 | |||
| 1ae3860612 | |||
| ae6afa9fbd |
1
.gitignore
vendored
1
.gitignore
vendored
@ -10,6 +10,7 @@
|
|||||||
*.lst
|
*.lst
|
||||||
*.ini
|
*.ini
|
||||||
*.iex
|
*.iex
|
||||||
|
*.pyc
|
||||||
*.sct
|
*.sct
|
||||||
*.scvd
|
*.scvd
|
||||||
*.uvguix
|
*.uvguix
|
||||||
|
|||||||
@ -8,7 +8,7 @@ OutputBaseFilename=MRobotInstaller
|
|||||||
|
|
||||||
[Files]
|
[Files]
|
||||||
; 复制整个 dist\MRobot 文件夹（onedir 模式生成的所有文件）
|
; 复制整个 dist\MRobot 文件夹（onedir 模式生成的所有文件）
|
||||||
Source: "dist\MRobot\*"; DestDir: "{app}"; Flags: ignoreversion recursesubdirs
|
Source: "dist\*"; DestDir: "{app}"; Flags: ignoreversion recursesubdirs
|
||||||
; 复制 assets 资源文件到安装目录（支持后续更新）
|
; 复制 assets 资源文件到安装目录（支持后续更新）
|
||||||
Source: "assets\logo\*"; DestDir: "{app}\assets\logo"; Flags: ignoreversion recursesubdirs
|
Source: "assets\logo\*"; DestDir: "{app}\assets\logo"; Flags: ignoreversion recursesubdirs
|
||||||
|
|
||||||
|
|||||||
@ -77,8 +77,29 @@ class BspSimplePeripheral(QWidget):
|
|||||||
return False
|
return False
|
||||||
output_path = os.path.join(self.project_path, f"User/bsp/{filename}")
|
output_path = os.path.join(self.project_path, f"User/bsp/{filename}")
|
||||||
CodeGenerator.save_with_preserve(output_path, template_content) # 使用保留用户区域的写入
|
CodeGenerator.save_with_preserve(output_path, template_content) # 使用保留用户区域的写入
|
||||||
|
|
||||||
|
# 复制BSP文件夹下的其他额外文件（如 README.md, CHANGELOG.md）
|
||||||
|
periph_template_dir = os.path.join(template_base_dir, periph_folder)
|
||||||
|
if os.path.exists(periph_template_dir) and os.path.isdir(periph_template_dir):
|
||||||
|
import shutil
|
||||||
|
files_to_process = list(self.template_names.values())
|
||||||
|
for item in os.listdir(periph_template_dir):
|
||||||
|
# 跳过已处理的主要文件
|
||||||
|
if item in files_to_process:
|
||||||
|
continue
|
||||||
|
|
||||||
|
src_file = os.path.join(periph_template_dir, item)
|
||||||
|
dst_file = os.path.join(self.project_path, f"User/bsp/{item}")
|
||||||
|
|
||||||
|
# 只复制文件，不复制子目录
|
||||||
|
if os.path.isfile(src_file):
|
||||||
|
os.makedirs(os.path.dirname(dst_file), exist_ok=True)
|
||||||
|
shutil.copy2(src_file, dst_file)
|
||||||
|
print(f"复制BSP额外文件: {item}")
|
||||||
|
|
||||||
self._save_config()
|
self._save_config()
|
||||||
return True
|
return True
|
||||||
|
return True
|
||||||
|
|
||||||
|
|
||||||
def _save_config(self):
|
def _save_config(self):
|
||||||
@ -405,6 +426,19 @@ class bsp_can(BspPeripheralBase):
|
|||||||
get_available_can
|
get_available_can
|
||||||
)
|
)
|
||||||
|
|
||||||
|
def _generate_bsp_code_internal(self):
|
||||||
|
"""重写生成方法，直接复制can文件夹中的文件"""
|
||||||
|
# 检查是否需要生成
|
||||||
|
if not self.is_need_generate():
|
||||||
|
for filename in self.template_names.values():
|
||||||
|
output_path = os.path.join(self.project_path, f"User/bsp/{filename}")
|
||||||
|
if os.path.exists(output_path):
|
||||||
|
return "skipped"
|
||||||
|
return "not_needed"
|
||||||
|
|
||||||
|
# 调用父类方法生成配置化的代码（头文件和源文件）
|
||||||
|
return super()._generate_bsp_code_internal()
|
||||||
|
|
||||||
def _generate_source_file(self, configs, template_dir):
|
def _generate_source_file(self, configs, template_dir):
|
||||||
# 从子文件夹加载模板（与_generate_header_file保持一致）
|
# 从子文件夹加载模板（与_generate_header_file保持一致）
|
||||||
periph_folder = self.peripheral_name.lower()
|
periph_folder = self.peripheral_name.lower()
|
||||||
@ -657,6 +691,37 @@ class bsp_fdcan(BspPeripheralBase):
|
|||||||
get_available_fdcan
|
get_available_fdcan
|
||||||
)
|
)
|
||||||
|
|
||||||
|
def _generate_bsp_code_internal(self):
|
||||||
|
"""重写生成方法，调用父类生成FDCAN代码后复制CAN兼容层"""
|
||||||
|
# 先调用父类方法生成FDCAN代码
|
||||||
|
result = super()._generate_bsp_code_internal()
|
||||||
|
if result and result not in ["skipped", "not_needed"]:
|
||||||
|
# 成功后复制CAN兼容层文件
|
||||||
|
self._copy_can_wrapper()
|
||||||
|
return result
|
||||||
|
|
||||||
|
def _copy_can_wrapper(self):
|
||||||
|
"""复制CAN兼容层文件(can.h)到项目"""
|
||||||
|
try:
|
||||||
|
template_base_dir = CodeGenerator.get_assets_dir("User_code/bsp")
|
||||||
|
fdcan_folder = os.path.join(template_base_dir, "fdcan")
|
||||||
|
bsp_output_dir = os.path.join(self.project_path, "User/bsp")
|
||||||
|
|
||||||
|
# 确保输出目录存在
|
||||||
|
os.makedirs(bsp_output_dir, exist_ok=True)
|
||||||
|
|
||||||
|
# 复制can.h（CAN兼容层头文件）
|
||||||
|
can_h_src = os.path.join(fdcan_folder, "can.h")
|
||||||
|
can_h_dst = os.path.join(bsp_output_dir, "can.h")
|
||||||
|
if os.path.exists(can_h_src):
|
||||||
|
with open(can_h_src, 'r', encoding='utf-8') as f:
|
||||||
|
content = f.read()
|
||||||
|
CodeGenerator.save_with_preserve(can_h_dst, content)
|
||||||
|
print(f"✓ 已复制CAN兼容层: can.h")
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(f"复制CAN兼容层文件时出错: {e}")
|
||||||
|
|
||||||
def _generate_header_file(self, configs, template_dir):
|
def _generate_header_file(self, configs, template_dir):
|
||||||
"""重写头文件生成，添加 FDCAN 使能和 FIFO 分配定义"""
|
"""重写头文件生成，添加 FDCAN 使能和 FIFO 分配定义"""
|
||||||
# 从子文件夹加载模板
|
# 从子文件夹加载模板
|
||||||
@ -1113,18 +1178,25 @@ class bsp_gpio(QWidget):
|
|||||||
)
|
)
|
||||||
|
|
||||||
# 生成EXTI使能代码 - 使用用户自定义的BSP枚举名称
|
# 生成EXTI使能代码 - 使用用户自定义的BSP枚举名称
|
||||||
|
# 根据引脚编号获取正确的IRQn（适配不同MCU）
|
||||||
enable_lines = []
|
enable_lines = []
|
||||||
disable_lines = []
|
disable_lines = []
|
||||||
for config in configs:
|
for config in configs:
|
||||||
if config['has_exti']:
|
if config['has_exti']:
|
||||||
ioc_label = config['ioc_label']
|
ioc_label = config['ioc_label']
|
||||||
custom_name = config['custom_name']
|
custom_name = config['custom_name']
|
||||||
|
# 尝试从IOC label中提取引脚编号，优先使用IOC定义的IRQn
|
||||||
enable_lines.append(f" case BSP_GPIO_{custom_name}:")
|
enable_lines.append(f" case BSP_GPIO_{custom_name}:")
|
||||||
|
enable_lines.append(f"#if defined({ioc_label}_EXTI_IRQn)")
|
||||||
enable_lines.append(f" HAL_NVIC_EnableIRQ({ioc_label}_EXTI_IRQn);")
|
enable_lines.append(f" HAL_NVIC_EnableIRQ({ioc_label}_EXTI_IRQn);")
|
||||||
enable_lines.append(f" break;")
|
enable_lines.append(f"#endif")
|
||||||
|
enable_lines.append(f" return BSP_OK;")
|
||||||
|
|
||||||
disable_lines.append(f" case BSP_GPIO_{custom_name}:")
|
disable_lines.append(f" case BSP_GPIO_{custom_name}:")
|
||||||
|
disable_lines.append(f"#if defined({ioc_label}_EXTI_IRQn)")
|
||||||
disable_lines.append(f" HAL_NVIC_DisableIRQ({ioc_label}_EXTI_IRQn);")
|
disable_lines.append(f" HAL_NVIC_DisableIRQ({ioc_label}_EXTI_IRQn);")
|
||||||
disable_lines.append(f" break;")
|
disable_lines.append(f"#endif")
|
||||||
|
disable_lines.append(f" return BSP_OK;")
|
||||||
|
|
||||||
content = CodeGenerator.replace_auto_generated(
|
content = CodeGenerator.replace_auto_generated(
|
||||||
content, "AUTO GENERATED BSP_GPIO_ENABLE_IRQ", "\n".join(enable_lines)
|
content, "AUTO GENERATED BSP_GPIO_ENABLE_IRQ", "\n".join(enable_lines)
|
||||||
|
|||||||
@ -124,6 +124,26 @@ class ComponentSimple(QWidget):
|
|||||||
continue
|
continue
|
||||||
output_path = os.path.join(self.project_path, f"User/component/{filename}")
|
output_path = os.path.join(self.project_path, f"User/component/{filename}")
|
||||||
CodeGenerator.save_with_preserve(output_path, template_content)
|
CodeGenerator.save_with_preserve(output_path, template_content)
|
||||||
|
|
||||||
|
# 复制组件文件夹下的其他额外文件
|
||||||
|
comp_template_dir = os.path.join(template_base_dir, comp_folder)
|
||||||
|
if os.path.exists(comp_template_dir) and os.path.isdir(comp_template_dir):
|
||||||
|
import shutil
|
||||||
|
files_to_process = list(self.template_names.values())
|
||||||
|
for item in os.listdir(comp_template_dir):
|
||||||
|
# 跳过已处理的主要文件
|
||||||
|
if item in files_to_process:
|
||||||
|
continue
|
||||||
|
|
||||||
|
src_file = os.path.join(comp_template_dir, item)
|
||||||
|
dst_file = os.path.join(self.project_path, f"User/component/{item}")
|
||||||
|
|
||||||
|
# 只复制文件，不复制子目录
|
||||||
|
if os.path.isfile(src_file):
|
||||||
|
os.makedirs(os.path.dirname(dst_file), exist_ok=True)
|
||||||
|
shutil.copy2(src_file, dst_file)
|
||||||
|
print(f"复制组件额外文件: {item}")
|
||||||
|
|
||||||
self._save_config()
|
self._save_config()
|
||||||
return True
|
return True
|
||||||
|
|
||||||
@ -271,75 +291,27 @@ class component(QWidget):
|
|||||||
if not dep_name.endswith(('.h', '.c', '.hpp', '.cpp')):
|
if not dep_name.endswith(('.h', '.c', '.hpp', '.cpp')):
|
||||||
components_to_generate.add(dep_name)
|
components_to_generate.add(dep_name)
|
||||||
|
|
||||||
# 为没有对应页面但需要生成的依赖组件创建临时页面
|
# 检查缺失的依赖组件
|
||||||
user_code_dir = CodeGenerator.get_assets_dir("User_code")
|
user_code_dir = CodeGenerator.get_assets_dir("User_code")
|
||||||
|
missing_dependencies = []
|
||||||
for comp_name in components_to_generate:
|
for comp_name in components_to_generate:
|
||||||
if comp_name not in component_pages:
|
if comp_name not in component_pages:
|
||||||
# 创建临时组件页面
|
missing_dependencies.append(comp_name)
|
||||||
template_names = {'header': f'{comp_name}.h', 'source': f'{comp_name}.c'}
|
|
||||||
temp_page = ComponentSimple(project_path, comp_name.upper(), template_names)
|
|
||||||
# temp_page.set_forced_enabled(True) # 自动启用依赖组件
|
|
||||||
component_pages[comp_name] = temp_page
|
|
||||||
|
|
||||||
# 如果没有组件需要生成，返回提示信息
|
# 如果没有组件需要生成，返回提示信息
|
||||||
if not components_to_generate:
|
if not components_to_generate:
|
||||||
return "没有启用的组件需要生成代码。"
|
return "没有启用的组件需要生成代码。"
|
||||||
|
|
||||||
# 生成代码和依赖文件
|
# 如果有缺失的依赖组件，提示用户
|
||||||
|
if missing_dependencies:
|
||||||
|
missing_msg = f"警告：以下依赖组件未启用，可能导致编译错误：{', '.join(missing_dependencies)}\n请在组件配置中启用这些依赖组件。\n\n"
|
||||||
|
else:
|
||||||
|
missing_msg = ""
|
||||||
|
|
||||||
|
# 生成代码
|
||||||
success_count = 0
|
success_count = 0
|
||||||
fail_count = 0
|
fail_count = 0
|
||||||
fail_list = []
|
fail_list = []
|
||||||
|
|
||||||
# 处理依赖文件的复制
|
|
||||||
all_deps = set()
|
|
||||||
for page in pages:
|
|
||||||
if hasattr(page, "component_name") and hasattr(page, "is_need_generate"):
|
|
||||||
if page.is_need_generate():
|
|
||||||
deps = page.get_enabled_dependencies()
|
|
||||||
all_deps.update(deps)
|
|
||||||
|
|
||||||
# 复制依赖文件
|
|
||||||
for dep_path in all_deps:
|
|
||||||
try:
|
|
||||||
# 检查是否是 bsp 层依赖
|
|
||||||
if dep_path.startswith('bsp/'):
|
|
||||||
# 对于 bsp 层依赖，跳过复制，因为这些由 BSP 代码生成负责
|
|
||||||
print(f"跳过 BSP 层依赖: {dep_path} (由 BSP 代码生成负责)")
|
|
||||||
continue
|
|
||||||
|
|
||||||
# dep_path 格式如 "component/filter" 或 "component/user_math.h"
|
|
||||||
src_path = os.path.join(user_code_dir, dep_path)
|
|
||||||
dst_path = os.path.join(project_path, "User", dep_path)
|
|
||||||
|
|
||||||
if os.path.isdir(src_path):
|
|
||||||
# 如果是目录，复制整个目录
|
|
||||||
os.makedirs(os.path.dirname(dst_path), exist_ok=True)
|
|
||||||
if os.path.exists(dst_path):
|
|
||||||
shutil.rmtree(dst_path)
|
|
||||||
shutil.copytree(src_path, dst_path)
|
|
||||||
elif os.path.isfile(src_path):
|
|
||||||
# 如果是文件，复制单个文件并保留用户区域
|
|
||||||
os.makedirs(os.path.dirname(dst_path), exist_ok=True)
|
|
||||||
with open(src_path, 'r', encoding='utf-8') as f:
|
|
||||||
new_content = f.read()
|
|
||||||
CodeGenerator.save_with_preserve(dst_path, new_content)
|
|
||||||
else:
|
|
||||||
# 如果既不是文件也不是目录，跳过
|
|
||||||
print(f"跳过不存在的依赖: {dep_path}")
|
|
||||||
continue
|
|
||||||
|
|
||||||
success_count += 1
|
|
||||||
print(f"成功复制依赖: {dep_path}")
|
|
||||||
except Exception as e:
|
|
||||||
# 对于 bsp 层依赖的错误，只记录但不计入失败
|
|
||||||
if dep_path.startswith('bsp/'):
|
|
||||||
print(f"BSP 层依赖 {dep_path} 复制失败，但忽略此错误: {e}")
|
|
||||||
else:
|
|
||||||
fail_count += 1
|
|
||||||
fail_list.append(f"{dep_path} (依赖复制异常: {e})")
|
|
||||||
print(f"复制依赖失败: {dep_path}, 错误: {e}")
|
|
||||||
|
|
||||||
# 生成组件代码
|
|
||||||
skipped_count = 0
|
skipped_count = 0
|
||||||
skipped_list = []
|
skipped_list = []
|
||||||
|
|
||||||
@ -370,8 +342,8 @@ class component(QWidget):
|
|||||||
fail_list.append(f"{comp_name} (生成异常: {e})")
|
fail_list.append(f"{comp_name} (生成异常: {e})")
|
||||||
print(f"生成组件异常: {comp_name}, 错误: {e}")
|
print(f"生成组件异常: {comp_name}, 错误: {e}")
|
||||||
|
|
||||||
total_items = len(all_deps) + len(components_to_generate)
|
total_items = len(components_to_generate)
|
||||||
msg = f"组件代码生成完成：总共处理 {total_items} 项，成功生成 {success_count} 项，跳过 {skipped_count} 项，失败 {fail_count} 项。"
|
msg = missing_msg + f"组件代码生成完成：总共处理 {total_items} 项，成功生成 {success_count} 项，跳过 {skipped_count} 项，失败 {fail_count} 项。"
|
||||||
if skipped_list:
|
if skipped_list:
|
||||||
msg += f"\n跳过项（文件已存在且未勾选）：\n" + "\n".join(skipped_list)
|
msg += f"\n跳过项（文件已存在且未勾选）：\n" + "\n".join(skipped_list)
|
||||||
if fail_list:
|
if fail_list:
|
||||||
|
|||||||
@ -278,11 +278,11 @@ class DeviceSimple(QWidget):
|
|||||||
with open(dst_path, 'w', encoding='utf-8') as f:
|
with open(dst_path, 'w', encoding='utf-8') as f:
|
||||||
f.write(content)
|
f.write(content)
|
||||||
|
|
||||||
# 复制设备文件夹下的其他文件（如 lcd_lib.h）
|
# 复制设备文件夹下的其他额外文件（如 lcd_lib.h）
|
||||||
if os.path.exists(device_template_dir):
|
if os.path.exists(device_template_dir):
|
||||||
import shutil
|
import shutil
|
||||||
for item in os.listdir(device_template_dir):
|
for item in os.listdir(device_template_dir):
|
||||||
# 跳过已处理的文件
|
# 跳过已处理的主要文件
|
||||||
if item in files_to_process:
|
if item in files_to_process:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
@ -291,11 +291,9 @@ class DeviceSimple(QWidget):
|
|||||||
|
|
||||||
# 只复制文件，不复制子目录
|
# 只复制文件，不复制子目录
|
||||||
if os.path.isfile(src_file):
|
if os.path.isfile(src_file):
|
||||||
# 检查文件是否已存在，避免覆盖
|
|
||||||
if not os.path.exists(dst_file):
|
|
||||||
os.makedirs(os.path.dirname(dst_file), exist_ok=True)
|
os.makedirs(os.path.dirname(dst_file), exist_ok=True)
|
||||||
shutil.copy2(src_file, dst_file)
|
shutil.copy2(src_file, dst_file)
|
||||||
print(f"复制额外文件: {dst_file}")
|
print(f"复制设备额外文件: {item}")
|
||||||
|
|
||||||
self._save_config()
|
self._save_config()
|
||||||
return True
|
return True
|
||||||
|
|||||||
BIN
assets/User_code/.DS_Store
vendored
BIN
assets/User_code/.DS_Store
vendored
Binary file not shown.
@ -1,113 +0,0 @@
|
|||||||
# User_code 目录结构说明
|
|
||||||
|
|
||||||
## 新的文件夹结构
|
|
||||||
|
|
||||||
所有外设、组件和设备的文件现在都存放在独立的子文件夹中，便于管理和维护。
|
|
||||||
|
|
||||||
### BSP (板级支持包)
|
|
||||||
```
|
|
||||||
bsp/
|
|
||||||
├── bsp.h # BSP 总头文件
|
|
||||||
├── describe.csv # 外设描述文件
|
|
||||||
├── .gitkeep
|
|
||||||
├── can/
|
|
||||||
│ ├── can.c
|
|
||||||
│ └── can.h
|
|
||||||
├── fdcan/
|
|
||||||
│ ├── fdcan.c
|
|
||||||
│ └── fdcan.h
|
|
||||||
├── uart/
|
|
||||||
│ ├── uart.c
|
|
||||||
│ └── uart.h
|
|
||||||
├── spi/
|
|
||||||
│ ├── spi.c
|
|
||||||
│ └── spi.h
|
|
||||||
├── i2c/
|
|
||||||
│ ├── i2c.c
|
|
||||||
│ └── i2c.h
|
|
||||||
├── gpio/
|
|
||||||
│ ├── gpio.c
|
|
||||||
│ └── gpio.h
|
|
||||||
├── pwm/
|
|
||||||
│ ├── pwm.c
|
|
||||||
│ └── pwm.h
|
|
||||||
├── time/
|
|
||||||
│ ├── time.c
|
|
||||||
│ └── time.h
|
|
||||||
├── dwt/
|
|
||||||
│ ├── dwt.c
|
|
||||||
│ └── dwt.h
|
|
||||||
└── mm/
|
|
||||||
├── mm.c
|
|
||||||
└── mm.h
|
|
||||||
```
|
|
||||||
|
|
||||||
### Component (组件)
|
|
||||||
```
|
|
||||||
component/
|
|
||||||
├── describe.csv # 组件描述文件
|
|
||||||
├── dependencies.csv # 组件依赖关系
|
|
||||||
├── .gitkeep
|
|
||||||
├── ahrs/
|
|
||||||
├── capacity/
|
|
||||||
├── cmd/
|
|
||||||
├── crc16/
|
|
||||||
├── crc8/
|
|
||||||
├── error_detect/
|
|
||||||
├── filter/
|
|
||||||
├── freertos_cli/
|
|
||||||
├── limiter/
|
|
||||||
├── mixer/
|
|
||||||
├── pid/
|
|
||||||
├── ui/
|
|
||||||
└── user_math/
|
|
||||||
```
|
|
||||||
|
|
||||||
### Device (设备)
|
|
||||||
```
|
|
||||||
device/
|
|
||||||
├── device.h # Device 总头文件
|
|
||||||
├── config.yaml # 设备配置文件
|
|
||||||
├── .gitkeep
|
|
||||||
├── bmi088/
|
|
||||||
├── buzzer/
|
|
||||||
├── dm_imu/
|
|
||||||
├── dr16/
|
|
||||||
├── ist8310/
|
|
||||||
├── led/
|
|
||||||
├── motor/
|
|
||||||
├── motor_dm/
|
|
||||||
├── motor_lk/
|
|
||||||
├── motor_lz/
|
|
||||||
├── motor_odrive/
|
|
||||||
├── motor_rm/
|
|
||||||
├── motor_vesc/
|
|
||||||
├── oid/
|
|
||||||
├── ops9/
|
|
||||||
├── rc_can/
|
|
||||||
├── servo/
|
|
||||||
├── vofa/
|
|
||||||
├── ws2812/
|
|
||||||
└── lcd_driver/ # LCD 驱动（原有结构）
|
|
||||||
```
|
|
||||||
|
|
||||||
## 代码生成逻辑
|
|
||||||
|
|
||||||
代码生成器会：
|
|
||||||
1. 首先尝试从子文件夹加载模板（如 `bsp/can/can.c`）
|
|
||||||
2. 如果子文件夹不存在，回退到根目录加载（向后兼容）
|
|
||||||
3. 生成时将文件展开到项目的扁平目录结构中（如 `User/bsp/can.c`）
|
|
||||||
|
|
||||||
## 优势
|
|
||||||
|
|
||||||
✅ **更好的组织**: 每个外设/组件的文件都在独立文件夹中
|
|
||||||
✅ **便于管理**: 添加、删除、修改模板更加方便
|
|
||||||
✅ **向后兼容**: 现有的扁平结构仍然可以正常工作
|
|
||||||
✅ **清晰的结构**: 一目了然地看到所有可用的外设/组件
|
|
||||||
|
|
||||||
## 迁移说明
|
|
||||||
|
|
||||||
如果你添加新的外设/组件/设备：
|
|
||||||
1. 在对应目录下创建新的子文件夹（小写命名）
|
|
||||||
2. 将 .c 和 .h 文件放入子文件夹
|
|
||||||
3. 代码生成器会自动识别并使用
|
|
||||||
79
assets/User_code/bsp/fdcan/can.h
Normal file
79
assets/User_code/bsp/fdcan/can.h
Normal file
@ -0,0 +1,79 @@
|
|||||||
|
/**
|
||||||
|
* @file can.h
|
||||||
|
* @brief CAN兼容层 - 将CAN接口映射到FDCAN
|
||||||
|
* @note 本文件用于FDCAN兼容CAN接口，设备层代码可以继续使用BSP_CAN_xxx接口
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "bsp/fdcan.h"
|
||||||
|
|
||||||
|
/* 类型映射 */
|
||||||
|
typedef BSP_FDCAN_t BSP_CAN_t;
|
||||||
|
typedef BSP_FDCAN_Callback_t BSP_CAN_Callback_t;
|
||||||
|
typedef BSP_FDCAN_Format_t BSP_CAN_Format_t;
|
||||||
|
typedef BSP_FDCAN_FrameType_t BSP_CAN_FrameType_t;
|
||||||
|
typedef BSP_FDCAN_Message_t BSP_CAN_Message_t;
|
||||||
|
typedef BSP_FDCAN_StdDataFrame_t BSP_CAN_StdDataFrame_t;
|
||||||
|
typedef BSP_FDCAN_ExtDataFrame_t BSP_CAN_ExtDataFrame_t;
|
||||||
|
typedef BSP_FDCAN_RemoteFrame_t BSP_CAN_RemoteFrame_t;
|
||||||
|
typedef BSP_FDCAN_IdParser_t BSP_CAN_IdParser_t;
|
||||||
|
|
||||||
|
/* 常量映射 */
|
||||||
|
#define BSP_CAN_MAX_DLC BSP_FDCAN_MAX_DLC
|
||||||
|
#define BSP_CAN_DEFAULT_QUEUE_SIZE BSP_FDCAN_DEFAULT_QUEUE_SIZE
|
||||||
|
#define BSP_CAN_TIMEOUT_IMMEDIATE BSP_FDCAN_TIMEOUT_IMMEDIATE
|
||||||
|
#define BSP_CAN_TIMEOUT_FOREVER BSP_FDCAN_TIMEOUT_FOREVER
|
||||||
|
#define BSP_CAN_TX_QUEUE_SIZE BSP_FDCAN_TX_QUEUE_SIZE
|
||||||
|
|
||||||
|
/* 枚举值映射 */
|
||||||
|
#define BSP_CAN_1 BSP_FDCAN_1
|
||||||
|
#define BSP_CAN_2 BSP_FDCAN_2
|
||||||
|
#define BSP_CAN_3 BSP_FDCAN_3
|
||||||
|
#define BSP_CAN_NUM BSP_FDCAN_NUM
|
||||||
|
#define BSP_CAN_ERR BSP_FDCAN_ERR
|
||||||
|
|
||||||
|
#define BSP_CAN_FORMAT_STD_DATA BSP_FDCAN_FORMAT_STD_DATA
|
||||||
|
#define BSP_CAN_FORMAT_EXT_DATA BSP_FDCAN_FORMAT_EXT_DATA
|
||||||
|
#define BSP_CAN_FORMAT_STD_REMOTE BSP_FDCAN_FORMAT_STD_REMOTE
|
||||||
|
#define BSP_CAN_FORMAT_EXT_REMOTE BSP_FDCAN_FORMAT_EXT_REMOTE
|
||||||
|
|
||||||
|
#define BSP_CAN_FRAME_STD_DATA BSP_FDCAN_FRAME_STD_DATA
|
||||||
|
#define BSP_CAN_FRAME_EXT_DATA BSP_FDCAN_FRAME_EXT_DATA
|
||||||
|
#define BSP_CAN_FRAME_STD_REMOTE BSP_FDCAN_FRAME_STD_REMOTE
|
||||||
|
#define BSP_CAN_FRAME_EXT_REMOTE BSP_FDCAN_FRAME_EXT_REMOTE
|
||||||
|
|
||||||
|
/* 函数映射 */
|
||||||
|
#define BSP_CAN_Init() BSP_FDCAN_Init()
|
||||||
|
#define BSP_CAN_GetHandle(can) BSP_FDCAN_GetHandle(can)
|
||||||
|
#define BSP_CAN_RegisterCallback(can, type, callback) \
|
||||||
|
BSP_FDCAN_RegisterCallback(can, type, callback)
|
||||||
|
#define BSP_CAN_Transmit(can, format, id, data, dlc) \
|
||||||
|
BSP_FDCAN_Transmit(can, format, id, data, dlc)
|
||||||
|
#define BSP_CAN_TransmitStdDataFrame(can, frame) \
|
||||||
|
BSP_FDCAN_TransmitStdDataFrame(can, frame)
|
||||||
|
#define BSP_CAN_TransmitExtDataFrame(can, frame) \
|
||||||
|
BSP_FDCAN_TransmitExtDataFrame(can, frame)
|
||||||
|
#define BSP_CAN_TransmitRemoteFrame(can, frame) \
|
||||||
|
BSP_FDCAN_TransmitRemoteFrame(can, frame)
|
||||||
|
#define BSP_CAN_RegisterId(can, can_id, queue_size) \
|
||||||
|
BSP_FDCAN_RegisterId(can, can_id, queue_size)
|
||||||
|
#define BSP_CAN_GetMessage(can, can_id, msg, timeout) \
|
||||||
|
BSP_FDCAN_GetMessage(can, can_id, msg, timeout)
|
||||||
|
#define BSP_CAN_GetQueueCount(can, can_id) \
|
||||||
|
BSP_FDCAN_GetQueueCount(can, can_id)
|
||||||
|
#define BSP_CAN_FlushQueue(can, can_id) \
|
||||||
|
BSP_FDCAN_FlushQueue(can, can_id)
|
||||||
|
#define BSP_CAN_RegisterIdParser(parser) \
|
||||||
|
BSP_FDCAN_RegisterIdParser(parser)
|
||||||
|
#define BSP_CAN_ParseId(original_id, frame_type) \
|
||||||
|
BSP_FDCAN_ParseId(original_id, frame_type)
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
@ -26,10 +26,18 @@ void BSP_Flash_EraseSector(uint32_t sector) {
|
|||||||
flash_erase.TypeErase = FLASH_TYPEERASE_SECTORS;
|
flash_erase.TypeErase = FLASH_TYPEERASE_SECTORS;
|
||||||
flash_erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
|
flash_erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
|
||||||
flash_erase.NbSectors = 1;
|
flash_erase.NbSectors = 1;
|
||||||
|
#if defined(STM32H7)
|
||||||
|
flash_erase.Banks = FLASH_BANK_1; // H7 requires Bank parameter
|
||||||
|
#endif
|
||||||
|
|
||||||
HAL_FLASH_Unlock();
|
HAL_FLASH_Unlock();
|
||||||
|
#if defined(STM32H7)
|
||||||
|
while (FLASH_WaitForLastOperation(50, FLASH_BANK_1) != HAL_OK)
|
||||||
|
;
|
||||||
|
#else
|
||||||
while (FLASH_WaitForLastOperation(50) != HAL_OK)
|
while (FLASH_WaitForLastOperation(50) != HAL_OK)
|
||||||
;
|
;
|
||||||
|
#endif
|
||||||
HAL_FLASHEx_Erase(&flash_erase, §or_error);
|
HAL_FLASHEx_Erase(&flash_erase, §or_error);
|
||||||
HAL_FLASH_Lock();
|
HAL_FLASH_Lock();
|
||||||
}
|
}
|
||||||
@ -39,6 +47,24 @@ void BSP_Flash_EraseSector(uint32_t sector) {
|
|||||||
|
|
||||||
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) {
|
void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) {
|
||||||
HAL_FLASH_Unlock();
|
HAL_FLASH_Unlock();
|
||||||
|
#if defined(STM32H7)
|
||||||
|
// H7 uses FLASHWORD (32 bytes) programming
|
||||||
|
uint8_t flash_word[32] __attribute__((aligned(32)));
|
||||||
|
while (len > 0) {
|
||||||
|
size_t chunk = (len < 32) ? len : 32;
|
||||||
|
memset(flash_word, 0xFF, 32);
|
||||||
|
memcpy(flash_word, buf, chunk);
|
||||||
|
|
||||||
|
while (FLASH_WaitForLastOperation(50, FLASH_BANK_1) != HAL_OK)
|
||||||
|
;
|
||||||
|
HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, address, (uint32_t)flash_word);
|
||||||
|
|
||||||
|
address += 32;
|
||||||
|
buf += chunk;
|
||||||
|
len -= chunk;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
// F4/F7 use byte programming
|
||||||
while (len > 0) {
|
while (len > 0) {
|
||||||
while (FLASH_WaitForLastOperation(50) != HAL_OK)
|
while (FLASH_WaitForLastOperation(50) != HAL_OK)
|
||||||
;
|
;
|
||||||
@ -47,6 +73,7 @@ void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) {
|
|||||||
buf++;
|
buf++;
|
||||||
len--;
|
len--;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
HAL_FLASH_Lock();
|
HAL_FLASH_Lock();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -69,7 +69,6 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
|
|||||||
default:
|
default:
|
||||||
return BSP_ERR;
|
return BSP_ERR;
|
||||||
}
|
}
|
||||||
return BSP_OK;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
|
int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
|
||||||
@ -78,7 +77,6 @@ int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
|
|||||||
default:
|
default:
|
||||||
return BSP_ERR;
|
return BSP_ERR;
|
||||||
}
|
}
|
||||||
return BSP_OK;
|
|
||||||
}
|
}
|
||||||
int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){
|
int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){
|
||||||
if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
|
if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
module_name,description
|
module_name,description
|
||||||
cmd,命令系统，用于机器人指令处理和行为控制
|
cmd,命令系统，用于机器人指令处理和行为控制
|
||||||
2_axis_gimbal,双轴云台控制模块，支持pitch和yaw轴控制
|
2_axis_gimbal,双轴云台控制模块，支持pitch和yaw轴控制
|
||||||
|
3+3shoot,双级三摩擦轮发射机构模块
|
||||||
|
658
assets/User_code/module/shoot/3+3shoot/shoot.c
Normal file
658
assets/User_code/module/shoot/3+3shoot/shoot.c
Normal file
@ -0,0 +1,658 @@
|
|||||||
|
/*
|
||||||
|
* far♂蛇模块
|
||||||
|
*/
|
||||||
|
|
||||||
|
/********************************* 使用示例 **********************************/
|
||||||
|
/*1.配置config参数以及Config_ShootInit函数参数*/
|
||||||
|
/*2.
|
||||||
|
COMP_AT9S_CMD_t shoot_ctrl_cmd_rc;
|
||||||
|
Shoot_t shoot;
|
||||||
|
Shoot_CMD_t shoot_cmd;
|
||||||
|
|
||||||
|
void Task(void *argument) {
|
||||||
|
|
||||||
|
Config_ShootInit();
|
||||||
|
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,SHOOT_CTRL_FREQ);
|
||||||
|
Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE); 关于模式选择：初始化一个模式
|
||||||
|
|
||||||
|
while (1) {
|
||||||
|
|
||||||
|
shoot_cmd.online =shoot_ctrl_cmd_rc.online;
|
||||||
|
shoot_cmd.ready =shoot_ctrl_cmd_rc.shoot.ready;
|
||||||
|
shoot_cmd.firecmd =shoot_ctrl_cmd_rc.shoot.firecmd;
|
||||||
|
|
||||||
|
shoot.mode =shoot_ctrl_cmd_rc.mode; 关于模式选择：或者用遥控器随时切换模式，二选一
|
||||||
|
|
||||||
|
Chassis_UpdateFeedback(&shoot);
|
||||||
|
Shoot_Control(&shoot,&shoot_cmd);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include "shoot.h"
|
||||||
|
#include "bsp/mm.h"
|
||||||
|
#include "bsp/time.h"
|
||||||
|
#include "component/filter.h"
|
||||||
|
#include "component/user_math.h"
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
#define MAX_FRIC_RPM 7000.0f
|
||||||
|
#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
static bool last_firecmd;
|
||||||
|
|
||||||
|
float maxTrigrpm=1500.0f;
|
||||||
|
/* Private function -------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 设置射击模式
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param mode 要设置的模式
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
s->mode=mode;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 重置PID积分
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_ResetIntegral(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i=0;i<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 SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 重置计算模块
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_ResetCalu(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i=0;i<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 SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 重置输出
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_ResetOutput(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i=0;i<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 SHOOT_OK;
|
||||||
|
}
|
||||||
|
//float last_angle=0.0f;
|
||||||
|
//float speed=0.0f;
|
||||||
|
//int8_t Shoot_CalufeedbackRPM(Shoot_t *s)
|
||||||
|
//{
|
||||||
|
// if (s == NULL) {
|
||||||
|
// return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
// }
|
||||||
|
//// static
|
||||||
|
// float err;
|
||||||
|
// err=CircleError(s->feedback.fric[0].rotor_abs_angle,last_angle,M_2PI);
|
||||||
|
// speed=err/s->dt/M_2PI*60.0f;
|
||||||
|
// last_angle=s->feedback.fric->rotor_abs_angle;
|
||||||
|
|
||||||
|
|
||||||
|
// return SHOOT_OK;
|
||||||
|
//}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 根据目标弹丸速度计算摩擦轮目标转速
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param target_speed 目标弹丸速度，单位m/s
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
switch(s->param->basic.projectileType)
|
||||||
|
{
|
||||||
|
case SHOOT_PROJECTILE_17MM:
|
||||||
|
s->target_variable.fric_rpm=5000.0f;
|
||||||
|
break;
|
||||||
|
case SHOOT_PROJECTILE_42MM:
|
||||||
|
s->target_variable.fric_rpm=4000.0f;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL || s->var_trig.num_toShoot == 0) {
|
||||||
|
return SHOOT_ERR_NULL;
|
||||||
|
}
|
||||||
|
float dt = s->timer.now - s->var_trig.time_lastShoot;
|
||||||
|
float dpos;
|
||||||
|
dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI);
|
||||||
|
if(dt >= 1.0f/s->param->basic.shot_freq && cmd->firecmd && dpos<=1.0f)
|
||||||
|
{
|
||||||
|
s->var_trig.time_lastShoot=s->timer.now;
|
||||||
|
CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI);
|
||||||
|
s->var_trig.num_toShoot--;
|
||||||
|
}
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static float Shoot_CaluCoupledWeight(Shoot_t *s, uint8_t fric_index)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
|
||||||
|
float Threshold;
|
||||||
|
switch (s->param->basic.projectileType) {
|
||||||
|
case SHOOT_PROJECTILE_17MM:
|
||||||
|
Threshold=50.0f;
|
||||||
|
break;
|
||||||
|
case SHOOT_PROJECTILE_42MM:
|
||||||
|
Threshold=400.0f;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float err;
|
||||||
|
err=fabs((s->param->basic.ratio_multilevel[fric_index]
|
||||||
|
*s->target_variable.fric_rpm)
|
||||||
|
-s->feedback.fric[fric_index].rotor_speed);
|
||||||
|
if (err<Threshold)
|
||||||
|
{
|
||||||
|
s->var_fric.coupled_control_weights=1.0f-(err*err)/(Threshold*Threshold);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
s->var_fric.coupled_control_weights=0.0f;
|
||||||
|
}
|
||||||
|
return s->var_fric.coupled_control_weights;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 更新射击模块的电机反馈信息
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_UpdateFeedback(Shoot_t *s)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
for(int i = 0; i < fric_num; i++) {
|
||||||
|
/* 更新摩擦轮电机反馈 */
|
||||||
|
MOTOR_RM_Update(&s->param->motor.fric[i].param);
|
||||||
|
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->motor.fric[i].param);
|
||||||
|
if(motor_fed!=NULL)
|
||||||
|
{
|
||||||
|
s->feedback.fric[i]=motor_fed->motor.feedback;
|
||||||
|
}
|
||||||
|
/* 滤波摩擦轮电机转速反馈 */
|
||||||
|
s->var_fric.fil_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
|
||||||
|
/* 归一化摩擦轮电机转速反馈 */
|
||||||
|
s->var_fric.normalized_fil_rpm[i] = s->var_fric.fil_rpm[i] / MAX_FRIC_RPM;
|
||||||
|
if(s->var_fric.normalized_fil_rpm[i]>1.0f)s->var_fric.normalized_fil_rpm[i]=1.0f;
|
||||||
|
if(s->var_fric.normalized_fil_rpm[i]<-1.0f)s->var_fric.normalized_fil_rpm[i]=-1.0f;
|
||||||
|
/* 计算平均摩擦轮电机转速反馈 */
|
||||||
|
s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1]+=s->var_fric.normalized_fil_rpm[i];
|
||||||
|
}
|
||||||
|
for (int i=1; i<MAX_NUM_MULTILEVEL; i++)
|
||||||
|
{
|
||||||
|
s->var_fric.normalized_fil_avgrpm[i]=s->var_fric.normalized_fil_avgrpm[i]/fric_num/MAX_NUM_MULTILEVEL;
|
||||||
|
}
|
||||||
|
/* 更新拨弹电机反馈 */
|
||||||
|
MOTOR_RM_Update(&s->param->motor.trig);
|
||||||
|
s->feedback.trig = *MOTOR_RM_GetMotor(&s->param->motor.trig);
|
||||||
|
s->var_trig.trig_agl=s->param->basic.extra_deceleration_ratio*s->feedback.trig.gearbox_total_angle;
|
||||||
|
while(s->var_trig.trig_agl<0)s->var_trig.trig_agl+=M_2PI;
|
||||||
|
while(s->var_trig.trig_agl>=M_2PI)s->var_trig.trig_agl-=M_2PI;
|
||||||
|
if (s->feedback.trig.motor.reverse) {
|
||||||
|
s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl;
|
||||||
|
}
|
||||||
|
s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed);
|
||||||
|
s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm;
|
||||||
|
if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f;
|
||||||
|
if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f;
|
||||||
|
|
||||||
|
s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 射击模块运行状态机
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/float a;
|
||||||
|
int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL || cmd == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = s->param->basic.fric_num;
|
||||||
|
static float pos;
|
||||||
|
if(s->mode==SHOOT_MODE_SAFE){
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
MOTOR_RM_Relax(&s->param->motor.fric[i].param);
|
||||||
|
}
|
||||||
|
MOTOR_RM_Relax(&s->param->motor.trig);\
|
||||||
|
pos=s->target_variable.trig_angle=s->var_trig.trig_agl;
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
switch(s->running_state)
|
||||||
|
{
|
||||||
|
case SHOOT_STATE_IDLE:/*熄火等待*/
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{ /* 转速归零 */
|
||||||
|
PID_ResetIntegral(&s->pid.fric_follow[i]);
|
||||||
|
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->var_fric.normalized_fil_rpm[i],0,s->timer.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->motor.fric[i].param, s->output.lpfout_fric[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt);
|
||||||
|
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt);
|
||||||
|
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
|
||||||
|
|
||||||
|
/* 检查状态机 */
|
||||||
|
if(cmd->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<fric_num;i++)
|
||||||
|
{
|
||||||
|
uint8_t level=s->param->motor.fric[i].level-1;
|
||||||
|
float target_rpm=s->param->basic.ratio_multilevel[level]
|
||||||
|
*s->target_variable.fric_rpm/MAX_FRIC_RPM;
|
||||||
|
/* 计算耦合控制权重 */
|
||||||
|
float w=Shoot_CaluCoupledWeight(s,i);
|
||||||
|
/* 计算跟随输出、计算修正输出 */
|
||||||
|
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
|
||||||
|
target_rpm,
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.out_err[i]=w*PID_Calc(&s->pid.fric_err[i],
|
||||||
|
s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1],
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.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->motor.fric[i].param, s->output.lpfout_fric[i]);
|
||||||
|
}
|
||||||
|
/* 设置拨弹电机输出 */
|
||||||
|
s->output.outagl_trig =PID_Calc(&s->pid.trig,
|
||||||
|
pos,
|
||||||
|
s->var_trig.trig_agl,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
|
||||||
|
s->output.outagl_trig,
|
||||||
|
s->var_trig.trig_rpm,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
|
||||||
|
|
||||||
|
/* 检查状态机 */
|
||||||
|
if(!cmd->ready)
|
||||||
|
{
|
||||||
|
Shoot_ResetCalu(s);
|
||||||
|
Shoot_ResetOutput(s);
|
||||||
|
s->running_state=SHOOT_STATE_IDLE;
|
||||||
|
}
|
||||||
|
else if(last_firecmd==false&&cmd->firecmd==true)
|
||||||
|
{
|
||||||
|
s->running_state=SHOOT_STATE_FIRE;
|
||||||
|
/* 根据模式设置待发射弹数 */
|
||||||
|
switch(s->mode)
|
||||||
|
{
|
||||||
|
case SHOOT_MODE_SINGLE:
|
||||||
|
s->var_trig.num_toShoot=1;
|
||||||
|
break;
|
||||||
|
case SHOOT_MODE_BURST:
|
||||||
|
s->var_trig.num_toShoot=s->param->basic.shot_burst_num;
|
||||||
|
break;
|
||||||
|
case SHOOT_MODE_CONTINUE:
|
||||||
|
s->var_trig.num_toShoot=6666;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
s->var_trig.num_toShoot=0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SHOOT_STATE_FIRE:/*射击*/
|
||||||
|
Shoot_CaluTargetAngle(s, cmd);
|
||||||
|
for(int i=0;i<fric_num;i++)
|
||||||
|
{
|
||||||
|
uint8_t level=s->param->motor.fric[i].level-1;
|
||||||
|
float target_rpm=s->param->basic.ratio_multilevel[level]
|
||||||
|
*s->target_variable.fric_rpm/MAX_FRIC_RPM;
|
||||||
|
/* 计算耦合控制权重 */
|
||||||
|
float w=Shoot_CaluCoupledWeight(s,i);
|
||||||
|
/* 计算跟随输出、计算修正输出 */
|
||||||
|
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
|
||||||
|
target_rpm,
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.out_err[i]=w*PID_Calc(&s->pid.fric_err[i],
|
||||||
|
s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1],
|
||||||
|
s->var_fric.normalized_fil_rpm[i],
|
||||||
|
0,
|
||||||
|
s->timer.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->motor.fric[i].param, s->output.lpfout_fric[i]);
|
||||||
|
}
|
||||||
|
/* 设置拨弹电机输出 */
|
||||||
|
s->output.outagl_trig =PID_Calc(&s->pid.trig,
|
||||||
|
s->target_variable.trig_angle,
|
||||||
|
s->var_trig.trig_agl,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
|
||||||
|
s->output.outagl_trig,
|
||||||
|
s->var_trig.trig_rpm,
|
||||||
|
0,
|
||||||
|
s->timer.dt);
|
||||||
|
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||||||
|
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
|
||||||
|
|
||||||
|
/* 检查状态机 */
|
||||||
|
if(!cmd->firecmd)
|
||||||
|
{
|
||||||
|
s->running_state=SHOOT_STATE_READY;
|
||||||
|
pos=s->var_trig.trig_agl;
|
||||||
|
s->var_trig.num_toShoot=0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
s->running_state=SHOOT_STATE_IDLE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/* 输出 */
|
||||||
|
MOTOR_RM_Ctrl(&s->param->motor.fric[0].param);
|
||||||
|
if(s->param->basic.fric_num>4)
|
||||||
|
{
|
||||||
|
MOTOR_RM_Ctrl(&s->param->motor.fric[4].param);
|
||||||
|
}
|
||||||
|
MOTOR_RM_Ctrl(&s->param->motor.trig);
|
||||||
|
last_firecmd = cmd->firecmd;
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 射击模块堵塞检测状态机
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
if(s->param->jamDetection.enable){
|
||||||
|
switch (s->jamdetection.fsmState) {
|
||||||
|
case SHOOT_JAMFSM_STATE_NORMAL:/* 正常运行 */
|
||||||
|
/* 检测电流是否超过阈值 */
|
||||||
|
if (s->feedback.trig.feedback.torque_current/1000.0f > s->param->jamDetection.threshold) {
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_SUSPECTED;
|
||||||
|
s->jamdetection.lastTime = s->timer.now; /* 记录怀疑开始时间 */
|
||||||
|
}
|
||||||
|
/* 正常运行射击状态机 */
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
break;
|
||||||
|
case SHOOT_JAMFSM_STATE_SUSPECTED:/* 怀疑堵塞 */
|
||||||
|
/* 检测电流是否低于阈值 */
|
||||||
|
if (s->feedback.trig.feedback.torque_current/1000.0f < s->param->jamDetection.threshold) {
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
/* 检测高阈值状态是否超过设定怀疑时间 */
|
||||||
|
else if ((s->timer.now - s->jamdetection.lastTime) >= s->param->jamDetection.suspectedTime) {
|
||||||
|
s->jamdetection.detected =true;
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_CONFIRMED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
/* 正常运行射击状态机 */
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
break;
|
||||||
|
case SHOOT_JAMFSM_STATE_CONFIRMED:/* 确认堵塞 */
|
||||||
|
/* 清空待发射弹 */
|
||||||
|
s->var_trig.num_toShoot=0;
|
||||||
|
/* 修改拨弹盘目标角度 */
|
||||||
|
s->target_variable.trig_angle = s->var_trig.trig_agl-(M_2PI/s->param->basic.num_trig_tooth);
|
||||||
|
/* 切换状态 */
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_DEAL;
|
||||||
|
/* 记录处理开始时间 */
|
||||||
|
s->jamdetection.lastTime = s->timer.now;
|
||||||
|
case SHOOT_JAMFSM_STATE_DEAL:/* 堵塞处理 */
|
||||||
|
/* 正常运行射击状态机 */
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
/* 给予0.3秒响应时间并检测电流小于20A，认为堵塞已解除 */
|
||||||
|
if ((s->timer.now - s->jamdetection.lastTime)>=0.3f&&s->feedback.trig.feedback.torque_current/1000.0f < 20.0f) {
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
|
||||||
|
s->jamdetection.detected = false;
|
||||||
|
Shoot_RunningFSM(s, cmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
/**
|
||||||
|
* \brief 初始化射击模块
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param param 包含射击参数的结构体
|
||||||
|
* \param target_freq 控制循环频率，单位Hz
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
||||||
|
{
|
||||||
|
if (s == NULL || param == NULL || target_freq <= 0.0f) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
uint8_t fric_num = param->basic.fric_num;
|
||||||
|
s->param=param;
|
||||||
|
BSP_CAN_Init();
|
||||||
|
/* 初始化摩擦轮PID和滤波器 */
|
||||||
|
for(int i=0;i<fric_num;i++){
|
||||||
|
MOTOR_RM_Register(¶m->motor.fric[i].param);
|
||||||
|
PID_Init(&s->pid.fric_follow[i],
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.fric_follow);
|
||||||
|
PID_Init(&s->pid.fric_err[i],
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.fric_err);
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
/* 初始化拨弹PID和滤波器 */
|
||||||
|
MOTOR_RM_Register(¶m->motor.trig);
|
||||||
|
switch(s->param->motor.trig.module)
|
||||||
|
{
|
||||||
|
case MOTOR_M3508:
|
||||||
|
PID_Init(&s->pid.trig,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_3508);
|
||||||
|
PID_Init(&s->pid.trig_omg,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_omg_3508);
|
||||||
|
break;
|
||||||
|
case MOTOR_M2006:
|
||||||
|
PID_Init(&s->pid.trig,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_2006);
|
||||||
|
PID_Init(&s->pid.trig_omg,
|
||||||
|
KPID_MODE_CALC_D,
|
||||||
|
target_freq,
|
||||||
|
¶m->pid.trig_omg_2006);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return SHOOT_ERR_MOTOR;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
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->var_trig,0,sizeof(s->var_trig));
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 射击模块控制主函数
|
||||||
|
*
|
||||||
|
* \param s 包含射击数据的结构体
|
||||||
|
* \param cmd 包含射击指令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
|
||||||
|
{
|
||||||
|
if (s == NULL || cmd == NULL) {
|
||||||
|
return SHOOT_ERR_NULL; // 参数错误
|
||||||
|
}
|
||||||
|
s->timer.now = BSP_TIME_Get_us() / 1000000.0f;
|
||||||
|
s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f;
|
||||||
|
s->timer.lask_wakeup = BSP_TIME_Get_us();
|
||||||
|
Shoot_CaluTargetRPM(s,233);
|
||||||
|
|
||||||
|
Shoot_JamDetectionFSM(s, cmd);
|
||||||
|
// Shoot_CalufeedbackRPM(s);
|
||||||
|
return SHOOT_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
243
assets/User_code/module/shoot/3+3shoot/shoot.h
Normal file
243
assets/User_code/module/shoot/3+3shoot/shoot.h
Normal file
@ -0,0 +1,243 @@
|
|||||||
|
/*
|
||||||
|
* far♂蛇模块
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stddef.h>
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include "main.h"
|
||||||
|
#include "component/pid.h"
|
||||||
|
#include "device/motor_rm.h"
|
||||||
|
/* Exported constants ------------------------------------------------------- */
|
||||||
|
#define MAX_FRIC_NUM 6
|
||||||
|
#define MAX_NUM_MULTILEVEL 2 /* 多级发射级数 */
|
||||||
|
|
||||||
|
#define SHOOT_OK (0) /* 运行正常 */
|
||||||
|
#define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */
|
||||||
|
#define SHOOT_ERR_ERR (-2) /* 运行时发现了其他错误 */
|
||||||
|
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的Mode */
|
||||||
|
#define SHOOT_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
|
||||||
|
#define SHOOT_ERR_MALLOC (-5) /* 内存分配失败 */
|
||||||
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_JAMFSM_STATE_NORMAL = 0,/* 常规状态 */
|
||||||
|
SHOOT_JAMFSM_STATE_SUSPECTED, /* 怀疑状态 */
|
||||||
|
SHOOT_JAMFSM_STATE_CONFIRMED, /* 确认状态 */
|
||||||
|
SHOOT_JAMFSM_STATE_DEAL /* 处理状态 */
|
||||||
|
}Shoot_JamDetectionFSM_State_t;
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_STATE_IDLE = 0,/* 熄火 */
|
||||||
|
SHOOT_STATE_READY, /* 准备射击 */
|
||||||
|
SHOOT_STATE_FIRE /* 射击 */
|
||||||
|
}Shoot_Running_State_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_MODE_SAFE = 0,/* 安全模式 */
|
||||||
|
SHOOT_MODE_SINGLE, /* 单发模式 */
|
||||||
|
SHOOT_MODE_BURST, /* 多发模式 */
|
||||||
|
SHOOT_MODE_CONTINUE,/* 连发模式 */
|
||||||
|
SHOOT_MODE_NUM
|
||||||
|
}Shoot_Mode_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
SHOOT_PROJECTILE_17MM,
|
||||||
|
SHOOT_PROJECTILE_42MM,
|
||||||
|
}Shoot_Projectile_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
MOTOR_RM_Param_t param;
|
||||||
|
uint8_t level; /* 电机属于第几级发射；1起始 */
|
||||||
|
}Shoot_MOTOR_RM_Param_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
MOTOR_Feedback_t fric[MAX_FRIC_NUM];/* 摩擦轮电机反馈 */
|
||||||
|
MOTOR_RM_t trig; /* 拨弹电机反馈 */
|
||||||
|
|
||||||
|
}Shoot_Feedback_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
float fil_rpm[MAX_FRIC_NUM]; /* 滤波后的摩擦轮原始转速 */
|
||||||
|
float normalized_fil_rpm[MAX_FRIC_NUM]; /* 归一化摩擦轮转速 */
|
||||||
|
float normalized_fil_avgrpm[MAX_NUM_MULTILEVEL]; /* 归一化摩擦轮平均转速 */
|
||||||
|
float coupled_control_weights; /* 耦合控制权重 */
|
||||||
|
}Shoot_VARSForFricCtrl_t;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
float time_lastShoot;/* 上次射击时间 */
|
||||||
|
uint16_t num_toShoot;/* 剩余待发射弹数 */
|
||||||
|
uint16_t num_shooted;/* 已发射弹数 */
|
||||||
|
|
||||||
|
float trig_agl; /*计算所有减速比后的拨弹盘的角度*/
|
||||||
|
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
|
||||||
|
float trig_rpm; /* 归一化拨弹电机转速 */
|
||||||
|
}Shoot_VARSForTrigCtrl_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
bool detected; /* 卡弹检测结果 */
|
||||||
|
float lastTime;/* 用于记录怀疑状态或处理状态的开始时间 */
|
||||||
|
Shoot_JamDetectionFSM_State_t fsmState; /* 卡弹检测状态机 */
|
||||||
|
}Shoot_JamDetection_t;
|
||||||
|
typedef struct {
|
||||||
|
float out_follow[MAX_FRIC_NUM];
|
||||||
|
float out_err[MAX_FRIC_NUM];
|
||||||
|
float out_fric[MAX_FRIC_NUM];
|
||||||
|
float lpfout_fric[MAX_FRIC_NUM];
|
||||||
|
|
||||||
|
float outagl_trig;
|
||||||
|
float outomg_trig;
|
||||||
|
float outlpf_trig;
|
||||||
|
}Shoot_Output_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
Shoot_Mode_t mode;/* 射击模式 */
|
||||||
|
bool ready; /* 准备射击 */
|
||||||
|
bool firecmd; /* 射击 */
|
||||||
|
}Shoot_CMD_t;
|
||||||
|
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||||||
|
typedef struct {
|
||||||
|
struct{
|
||||||
|
Shoot_Projectile_t projectileType; /* 发射弹丸类型 */;
|
||||||
|
size_t fric_num; /* 摩擦轮电机数量 */
|
||||||
|
float ratio_multilevel[MAX_NUM_MULTILEVEL]; /* 多级发射各级速度比例 */
|
||||||
|
float extra_deceleration_ratio; /*电机出轴到拨盘的额外减速比；没有写1*/
|
||||||
|
size_t num_trig_tooth; /* 拨弹盘每圈弹丸数量 */
|
||||||
|
float shot_freq; /* 射击频率，单位Hz */
|
||||||
|
size_t shot_burst_num; /* 多发模式一次射击的数量 */
|
||||||
|
}basic;/* 发射基础参数 */
|
||||||
|
struct {
|
||||||
|
bool enable; /* 是否启用卡弹检测 */
|
||||||
|
float threshold; /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为235A,根据实际测试调整)*/
|
||||||
|
float suspectedTime;/* 卡弹怀疑时间，单位秒 */
|
||||||
|
}jamDetection;/* 卡弹检测参数 */
|
||||||
|
struct {
|
||||||
|
Shoot_MOTOR_RM_Param_t fric[MAX_FRIC_NUM];
|
||||||
|
MOTOR_RM_Param_t trig;
|
||||||
|
}motor; /* 电机参数 */
|
||||||
|
struct{
|
||||||
|
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
|
||||||
|
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
|
||||||
|
KPID_Params_t trig_2006; /* 拨弹电机PID控制参数 */
|
||||||
|
KPID_Params_t trig_omg_2006;/* 拨弹电机PID控制参数 */
|
||||||
|
KPID_Params_t trig_3508; /* 拨弹电机PID控制参数 */
|
||||||
|
KPID_Params_t trig_omg_3508;/* 拨弹电机PID控制参数 */
|
||||||
|
}pid; /* PID参数 */
|
||||||
|
struct {
|
||||||
|
struct{
|
||||||
|
float in; /* 反馈值滤波器截止频率 */
|
||||||
|
float out; /* 输出值滤波器截止频率 */
|
||||||
|
}fric;
|
||||||
|
struct{
|
||||||
|
float in; /* 反馈值滤波器截止频率 */
|
||||||
|
float out; /* 输出值滤波器截止频率 */
|
||||||
|
}trig;
|
||||||
|
} filter;/* 滤波器截止频率参数 */
|
||||||
|
} Shoot_Params_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
float now; /* 当前时间，单位秒 */
|
||||||
|
uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
|
||||||
|
float dt; /* 两次唤醒间隔时间，单位秒 */
|
||||||
|
}Shoot_Timer_t;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
||||||
|
* 包含了初始化参数，中间变量，输出变量
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
Shoot_Timer_t timer; /* 计时器 */
|
||||||
|
Shoot_Params_t *param; /* 发射参数 */
|
||||||
|
/* 模块通用 */
|
||||||
|
Shoot_Mode_t mode; /* 射击模式 */
|
||||||
|
/* 反馈信息 */
|
||||||
|
Shoot_Feedback_t feedback;
|
||||||
|
/* 控制信息*/
|
||||||
|
Shoot_Running_State_t running_state; /* 运行状态机 */
|
||||||
|
Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */
|
||||||
|
Shoot_VARSForFricCtrl_t var_fric; /* 摩擦轮控制信息 */
|
||||||
|
Shoot_VARSForTrigCtrl_t var_trig; /* 角度计算控制信息 */
|
||||||
|
Shoot_Output_t output; /* 输出信息 */
|
||||||
|
/* 目标控制量 */
|
||||||
|
struct {
|
||||||
|
float fric_rpm; /* 目标摩擦轮转速 */
|
||||||
|
float trig_angle;/* 目标拨弹位置 */
|
||||||
|
}target_variable;
|
||||||
|
|
||||||
|
/* 反馈控制用的PID */
|
||||||
|
struct {
|
||||||
|
KPID_t fric_follow[MAX_FRIC_NUM];/* 摩擦轮PID主结构体 */
|
||||||
|
KPID_t fric_err[MAX_FRIC_NUM]; /* 摩擦轮PID主结构体 */
|
||||||
|
KPID_t trig; /* 拨弹PID主结构体 */
|
||||||
|
KPID_t trig_omg; /* 拨弹PID主结构体 */
|
||||||
|
} pid;
|
||||||
|
|
||||||
|
/* 滤波器 */
|
||||||
|
struct {
|
||||||
|
struct{
|
||||||
|
LowPassFilter2p_t in[MAX_FRIC_NUM]; /* 反馈值滤波器 */
|
||||||
|
LowPassFilter2p_t out[MAX_FRIC_NUM];/* 输出值滤波器 */
|
||||||
|
}fric;
|
||||||
|
struct{
|
||||||
|
LowPassFilter2p_t in; /* 反馈值滤波器 */
|
||||||
|
LowPassFilter2p_t out;/* 输出值滤波器 */
|
||||||
|
}trig;
|
||||||
|
} filter;
|
||||||
|
|
||||||
|
float errtosee; /*调试用*/
|
||||||
|
|
||||||
|
} Shoot_t;
|
||||||
|
|
||||||
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 初始化发射
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param param 包含发射参数的结构体指针
|
||||||
|
* \param target_freq 任务预期的运行频率
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 设置发射模式
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param mode 包含发射模式的枚举
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 更新反馈
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_UpdateFeedback(Shoot_t *s);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief 初始化发射
|
||||||
|
*
|
||||||
|
* \param s 包含发射数据的结构体
|
||||||
|
* \param cmd 包含发射命令的结构体
|
||||||
|
*
|
||||||
|
* \return 函数运行结果
|
||||||
|
*/
|
||||||
|
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user