F - Count the Colors

Painting some colored segments on a line, some previously painted segments may be covered by some the subsequent ones.

Your task is counting the segments of different colors you can see at last.

 

Input



The first line of each data set contains exactly one integer n, 1 <= n <= 8000, equal to the number of colored segments.

 

Each of the following n lines consists of exactly 3 nonnegative integers separated by single spaces:

x1 x2 c

x1 and x2 indicate the left endpoint and right endpoint of the segment, c indicates the color of the segment.

All the numbers are in the range [0, 8000], and they are all integers.

Input may contain several data set, process to the end of file.

 

Output



Each line of the output should contain a color index that can be seen from the top, following the count of the segments of this color, they should be printed according to the color index.

 

If some color can't be seen, you shouldn't print it.

Print a blank line after every dataset.

 

Sample Input



5
0 4 4
0 3 1
3 4 2
0 2 2
0 2 3
4
0 1 1
3 4 1
1 3 2
1 3 1
6
0 1 0
1 2 1
2 3 1
1 2 0
2 3 0
1 2 1

 

 

Sample Output



1 1
2 1
3 1

 

1 1

0 2
1 1

#include<algorithm>
#include<cstdio>
#include<cstring>
#include<cstdlib>
#include<vector>
#include<queue>
#include<stack>
#include<iomanip>
#include<string>
#include<climits>
#include<cmath>
#define INF 0x3f3f3f3f
#define MAXN 8010
#define LL long long
using namespace std;
struct node{
    int l,r;
	int color;
};
node segTree[4*MAXN];

void build(int num,int l,int r)
{
	segTree[num].l=l;
	segTree[num].r=r;
	segTree[num].color=-1;
	//确定num的l和r以及color
	if(l==r)
		return;
	int mid=(l+r)>>1;
	build(num<<1,l,mid);
	build(num<<1|1,mid+1,r);
}

void pushdown(int num)
{
	segTree[num<<1].color=segTree[num<<1|1].color=segTree[num].color;
	segTree[num].color=-1;
}
void update(int num,int l,int r,int c)
{
	if(segTree[num].l>=l&&segTree[num].r<=r)
    {
    	segTree[num].color=c;
    	return;
    }
    if(segTree[num].l>r||segTree[num].r<l) return;
    if(segTree[num].color!=-1) pushdown(num);
    update(num<<1,l,r,c);
    update(num<<1|1,l,r,c);
}

int mark[MAXN],cnt;

void query(int l,int r,int x)
{
    if((l==segTree[x].l&&segTree[x].r==r&&segTree[x].color!=-1)||segTree[x].l==segTree[x].r)
    {
        mark[cnt++]=segTree[x].color;
        return;
    }
    LL tmp=x<<1;
    LL mid=(segTree[x].l+segTree[x].r)>>1;
    if(r<=mid)   return query(l,r,tmp);
    else if(l>mid)  return query(l,r,tmp+1);
    else
    {
        query(l,mid,tmp);
        query(mid+1,r,tmp+1);
    }
}

int main()
{
	int n;
	while(~scanf("%d",&n))
    {
        memset(mark,-1,sizeof(mark));
        int mmax=0;
        int x1[MAXN<<2],x2[MAXN<<2],c[MAXN<<2],sum[MAXN<<2];
        memset(sum,0,sizeof(sum));
        for(int i=1;i<=n;i++)
        {
            scanf("%d%d%d",&x1[i],&x2[i],&c[i]);
            if(mmax<x2[i]) mmax=x2[i];
        }
        build(1,1,mmax);
        for(int i=1;i<=n;i++)
            update(1,x1[i]+1,x2[i],c[i]);
        query(1,mmax,1);
        for(int i=0;i<cnt;)
        {
            if(mark[i]==-1) {i++;continue;}
            else
            {
                int x=mark[i];
                while(mark[++i]==x&&i<cnt);
                sum[x]++;
            }
        }
        for(int i=0;i<8010;i++)
            if(sum[i])
         printf("%d %d\n",i,sum[i]);
        printf("\n");
    }
    return 0;
}

 

根据我下边的代码修改,这段代码我在R-CNN可以正常使用,改为ssd使用。训练好的模型为ssd模型,使用vgg16,图像和标注在YOLO数据集上,我要数据,比如map50,map50-95,精度P,召回率R,参数量Parameters,计算量GFLOPs,检测速度FPS。生成一段可以直接使用的代码 import os import sys import json import torch import numpy as np import pandas as pd import time import argparse import logging from pathlib import Path from PIL import Image from tqdm import tqdm import glob import shutil import warnings from collections import defaultdict # 忽略特定警告 warnings.filterwarnings('ignore') # ============================================== # 1. 修复路径问题 - 自动查找SSD项目 # ============================================== def find_ssd_project(): """查找SSD项目目录""" current_dir = os.path.dirname(os.path.abspath(__file__)) # 可能的项目路径 possible_project_dirs = [ # 在当前目录或其父目录中查找 current_dir, os.path.dirname(current_dir), os.path.dirname(os.path.dirname(current_dir)), # 特定名称的目录 os.path.join(os.path.dirname(current_dir), 'ssd-pytorch-master'), os.path.join(current_dir, 'ssd-pytorch-master'), # 您的特定路径 'C:/A/ZY/0_YOLO资源包/1_环境配置/ssd-pytorch-master', '../ssd-pytorch-master', '../../ssd-pytorch-master', ] # 检查每个可能的目录 for project_dir in possible_project_dirs: if os.path.exists(project_dir): # 检查是否包含必要的文件 ssd_file = os.path.join(project_dir, 'ssd.py') nets_dir = os.path.join(project_dir, 'nets') # SSD项目通常有ssd.py或nets/ssd.py if os.path.exists(nets_dir) or os.path.exists(ssd_file): print(f"找到SSD项目目录: {project_dir}") return project_dir return None # 查找并添加项目目录 project_dir = find_ssd_project() if project_dir: # 添加到Python路径 if project_dir not in sys.path: sys.path.insert(0, project_dir) else: print("错误: 未找到SSD项目目录") print("请确保您在正确的目录中运行") print("\n当前目录:", os.getcwd()) sys.exit(1) # ============================================== # 2. 导入SSD项目模块 # ============================================== try: # 尝试导入SSD模型(根据您的SSD项目结构) from nets.ssd import SSD print("成功导入SSD模块") # 假设您的SSD类初始化方式 # 可能需要根据实际项目调整 SSD_MODEL = SSD except ImportError: try: # 另一种可能的导入方式 from ssd import SSD SSD_MODEL = SSD print("成功导入SSD模块") except ImportError: try: # 有些项目可能这样组织 from model.ssd import SSD SSD_MODEL = SSD print("成功导入SSD模块") except ImportError as e: print(f"导入SSD模块失败: {e}") print("请检查SSD模型的文件结构") # 列出当前目录文件 print("\n当前目录内容:") for file in os.listdir('.'): if file.endswith('.py'): print(f" {file}") print("\nnets目录内容:") nets_dir = os.path.join(project_dir, 'nets') if os.path.exists(nets_dir): for file in os.listdir(nets_dir): if file.endswith('.py'): print(f" {file}") sys.exit(1) try: # 尝试导入utils模块 from utils.utils import get_classes from utils.utils_map import get_map print("成功导入utils模块") except ImportError as e: print(f"导入utils模块失败: {e}") # 尝试直接导入 import importlib.util # 尝试导入get_classes utils_path = os.path.join(project_dir, "utils", "utils.py") if os.path.exists(utils_path): spec = importlib.util.spec_from_file_location("utils", utils_path) utils_module = importlib.util.module_from_spec(spec) spec.loader.exec_module(utils_module) get_classes = utils_module.get_classes print("成功直接导入get_classes") else: print(f"未找到utils.py: {utils_path}") sys.exit(1) # 尝试导入get_map utils_map_path = os.path.join(project_dir, "utils", "utils_map.py") if os.path.exists(utils_map_path): spec2 = importlib.util.spec_from_file_location("utils_map", utils_map_path) utils_map_module = importlib.util.module_from_spec(spec2) spec2.loader.exec_module(utils_map_module) get_map = utils_map_module.get_map print("成功直接导入get_map") else: print(f"未找到utils_map.py: {utils_map_path}") sys.exit(1) # ============================================== # 3. SSD评估函数 # ============================================== def setup_logging(output_dir): """设置日志记录""" os.makedirs(output_dir, exist_ok=True) log_path = os.path.join(output_dir, 'ssd_evaluation.log') logger = logging.getLogger('SSD_Evaluation') logger.setLevel(logging.INFO) # 清除现有的handlers logger.handlers.clear() # 文件handler file_handler = logging.FileHandler(log_path, encoding='utf-8') file_handler.setLevel(logging.INFO) # 控制台handler console_handler = logging.StreamHandler() console_handler.setLevel(logging.INFO) # 格式化器 formatter = logging.Formatter('%(asctime)s - %(levelname)s - %(message)s') file_handler.setFormatter(formatter) console_handler.setFormatter(formatter) # 添加handlers logger.addHandler(file_handler) logger.addHandler(console_handler) return logger def calculate_model_params(model): """计算模型参数量""" try: total_params = sum(p.numel() for p in model.parameters()) trainable_params = sum(p.numel() for p in model.parameters() if p.requires_grad) except: total_params = 0 trainable_params = 0 return total_params, trainable_params def load_val_images(yolo_data_path): """加载val集的图像""" val_images = [] # 尝试多个可能的路径模式 possible_paths = [ os.path.join(yolo_data_path, 'val.txt'), os.path.join(yolo_data_path, 'images/val'), os.path.join(yolo_data_path, 'val/images'), os.path.join(yolo_data_path, 'val'), os.path.join(yolo_data_path, 'images'), yolo_data_path ] for path in possible_paths: if os.path.exists(path): if path.endswith('.txt'): with open(path, 'r') as f: for line in f: line = line.strip() if line: val_images.append(line) if val_images: return val_images else: # 加载图像文件 image_extensions = ['*.jpg', '*.jpeg', '*.png', '*.bmp'] for ext in image_extensions: val_images.extend(glob.glob(os.path.join(path, '**', ext), recursive=True)) val_images.extend(glob.glob(os.path.join(path, ext))) if val_images: return val_images return val_images def find_image_full_path(yolo_data_path, img_record): """根据记录查找完整的图像路径""" img_record = img_record.strip() # 如果已经是绝对路径且存在 if os.path.isabs(img_record) and os.path.exists(img_record): return img_record # 尝试多个可能的路径 possible_paths = [ os.path.join(yolo_data_path, img_record), os.path.join(yolo_data_path, 'images', img_record), os.path.join(yolo_data_path, 'images', 'val', img_record), os.path.join(yolo_data_path, 'val', 'images', img_record), os.path.join(yolo_data_path, 'val', img_record), ] for test_path in possible_paths: if os.path.exists(test_path): return test_path # 如果记录中没有扩展名,尝试添加常见扩展名 filename, ext = os.path.splitext(img_record) if not ext: for ext in ['.jpg', '.jpeg', '.png', '.bmp']: test_paths = [ os.path.join(yolo_data_path, 'images', 'val', filename + ext), os.path.join(yolo_data_path, 'images', filename + ext), os.path.join(yolo_data_path, filename + ext), os.path.join(yolo_data_path, 'val', 'images', filename + ext), ] for test_path in test_paths: if os.path.exists(test_path): return test_path return None def find_label_file(yolo_data_path, img_path): """查找对应的标签文件""" # 获取图像文件名(不含扩展名) img_filename = os.path.splitext(os.path.basename(img_path))[0] # 获取图像的相对路径(相对于images目录) if 'images' in img_path: img_rel_path = img_path.split('images')[1].lstrip(os.sep) img_rel_dir = os.path.dirname(img_rel_path) else: img_rel_dir = '' # 尝试不同的标签路径 possible_label_paths = [ img_path.replace('images', 'labels').replace(os.path.splitext(img_path)[1], '.txt'), os.path.join(yolo_data_path, 'labels', 'val', img_filename + '.txt'), os.path.join(yolo_data_path, 'labels', img_filename + '.txt'), os.path.join(yolo_data_path, 'labels', img_rel_dir, img_filename + '.txt'), os.path.join(yolo_data_path, 'val', 'labels', img_filename + '.txt'), ] for label_path in possible_label_paths: if os.path.exists(label_path): return label_path return None def evaluate_ssd_val(model, classes_path, yolo_data_path, output_dir='ssd_evaluation_results_val', confidence=0.5, nms_iou=0.5, max_images=None, logger=None): """ 使用val集评估SSD模型 """ # 创建输出目录 os.makedirs(output_dir, exist_ok=True) if logger is None: logger = logging.getLogger('SSD_Evaluation') # 获取类别信息 class_names, _ = get_classes(classes_path) num_classes = len(class_names) logger.info(f"类别数量: {num_classes}") logger.info(f"类别列表: {class_names}") # 1. 计算模型参数量 logger.info("计算模型参数量...") total_params, trainable_params = calculate_model_params(model) logger.info(f"模型参数量统计:") logger.info(f" 总参数量: {total_params:,}") logger.info(f" 可训练参数量: {trainable_params:,}") # 2. 加载val集图像 logger.info("加载val集图像...") val_image_records = load_val_images(yolo_data_path) if not val_image_records: logger.error("未找到val集图像!") return None logger.info(f"找到 {len(val_image_records)} 个val集记录") # 转换为完整路径 val_images = [] for record in tqdm(val_image_records, desc="解析图像路径"): full_path = find_image_full_path(yolo_data_path, record) if full_path and os.path.exists(full_path): val_images.append(full_path) else: logger.warning(f"无法找到图像: {record}") logger.info(f"成功解析 {len(val_images)} 个val集图像路径") if not val_images: logger.error("没有找到有效的图像文件!") return None # 限制测试图像数量 if max_images and max_images < len(val_images): test_images = val_images[:max_images] logger.info(f"限制测试 {max_images} 张图像") else: test_images = val_images logger.info(f"测试全部 {len(test_images)} 张图像") # 3. 准备评估数据结构 all_predictions = [] all_ground_truths = [] inference_times = [] # 4. 创建临时目录 temp_dir = os.path.join(output_dir, 'temp_mAP') # 删除旧目录(如果存在) if os.path.exists(temp_dir): shutil.rmtree(temp_dir, ignore_errors=True) os.makedirs(temp_dir, exist_ok=True) os.makedirs(os.path.join(temp_dir, 'ground-truth'), exist_ok=True) os.makedirs(os.path.join(temp_dir, 'detection-results'), exist_ok=True) logger.info("开始推理...") # 5. 进行推理 for img_path in tqdm(test_images, desc="推理进度"): try: # 获取图像ID image_id = os.path.splitext(os.path.basename(img_path))[0] # 加载图像 image = Image.open(img_path) image = image.convert('RGB') img_width, img_height = image.size # 推理时间测量 start_time = time.time() # 使用SSD模型进行检测 # 根据您的SSD项目,这里可能需要调整 if hasattr(model, 'detect_image'): # 假设SSD模型有detect_image方法 r_image, predictions = model.detect_image(image) # 保存检测结果到文件 det_file = os.path.join(temp_dir, 'detection-results', f"{image_id}.txt") with open(det_file, 'w') as f: for pred in predictions: # 假设predictions的格式为 [x1, y1, x2, y2, class_id, confidence] if len(pred) >= 6: class_id = int(pred[4]) confidence_score = float(pred[5]) if class_id < num_classes: class_name = class_names[class_id] x1, y1, x2, y2 = pred[:4] f.write(f"{class_name} {confidence_score:.6f} {x1:.1f} {y1:.1f} {x2:.1f} {y2:.1f}\n") inference_time = time.time() - start_time inference_times.append(inference_time) # 查找对应的标签文件 label_path = find_label_file(yolo_data_path, img_path) if label_path and os.path.exists(label_path): with open(label_path, 'r') as f: lines = f.readlines() for line in lines: parts = line.strip().split() if len(parts) >= 5: try: class_id = int(parts[0]) x_center = float(parts[1]) * img_width y_center = float(parts[2]) * img_height width = float(parts[3]) * img_width height = float(parts[4]) * img_height x1 = max(0, x_center - width / 2) y1 = max(0, y_center - height / 2) x2 = min(img_width, x_center + width / 2) y2 = min(img_height, y_center + height / 2) if 0 <= class_id < num_classes: all_ground_truths.append({ 'image_id': image_id, 'bbox': [x1, y1, x2, y2], 'category_id': class_id, 'category_name': class_names[class_id] }) # 保存ground truth到文件 gt_file = os.path.join(temp_dir, 'ground-truth', f"{image_id}.txt") with open(gt_file, 'a') as f: f.write(f"{class_names[class_id]} {x1:.1f} {y1:.1f} {x2:.1f} {y2:.1f}\n") except Exception as e: logger.warning(f"解析标签行时出错: {line}, 错误: {e}") continue else: logger.warning(f"未找到标签文件: {img_path}") except Exception as e: logger.error(f"处理图像 {img_path} 时出错: {e}") continue logger.info(f"成功处理 {len(inference_times)} 张图像") logger.info(f"真实标注有 {len(all_ground_truths)} 个目标") if len(inference_times) == 0: logger.error("没有成功处理任何图像!") return None # 6. 确保所有图像都有对应的文件 logger.info("确保所有文件存在...") # 为每个图像创建空的检测结果文件(如果不存在) for img_path in test_images: image_id = os.path.splitext(os.path.basename(img_path))[0] det_file = os.path.join(temp_dir, 'detection-results', f"{image_id}.txt") if not os.path.exists(det_file): with open(det_file, 'w') as f: pass # 7. 计算FPS avg_inference_time = np.mean(inference_times) fps = 1.0 / avg_inference_time if avg_inference_time > 0 else 0 logger.info(f"推理速度:") logger.info(f" 平均推理时间: {avg_inference_time * 1000:.2f}ms") logger.info(f" FPS: {fps:.2f}") # 8. 计算mAP50 logger.info("计算mAP50指标...") map50 = 0.0 try: map50 = get_map(0.5, False, score_threhold=0.5, path=temp_dir) logger.info(f" mAP50: {map50:.4f}") except Exception as e: logger.error(f"计算mAP50时出错: {e}") map50 = 0.0 # 9. 生成评估报告 evaluation_report = { 'model_info': { 'total_parameters': int(total_params), 'trainable_parameters': int(trainable_params), 'confidence_threshold': confidence, 'nms_iou_threshold': nms_iou }, 'performance_metrics': { 'mAP50': float(map50) }, 'inference_speed': { 'avg_inference_time_ms': float(avg_inference_time * 1000), 'fps': float(fps), 'test_images_count': len(inference_times) }, 'dataset_info': { 'val_set_size': len(val_images), 'actual_tested_count': len(inference_times), 'num_classes': num_classes, 'class_names': class_names }, 'detection_counts': { 'total_ground_truths': len(all_ground_truths), 'average_ground_truths_per_image': len(all_ground_truths) / len(inference_times) if inference_times else 0 } } # 10. 保存结果 logger.info("保存评估结果...") # 保存为JSON json_path = os.path.join(output_dir, 'ssd_evaluation_results.json') with open(json_path, 'w', encoding='utf-8') as f: json.dump(evaluation_report, f, indent=4, ensure_ascii=False, default=str) logger.info(f" JSON结果保存至: {json_path}") # 保存为CSV csv_data = [] main_metrics = [ ('模型参数量', evaluation_report['model_info']['total_parameters'], 'Parameters'), ('mAP50', evaluation_report['performance_metrics']['mAP50'], ''), ('FPS', evaluation_report['inference_speed']['fps'], ''), ('推理时间', evaluation_report['inference_speed']['avg_inference_time_ms'], 'ms'), ] for name, value, unit in main_metrics: csv_data.append({ '指标类别': '主要指标', '指标名称': name, '数值': value, '单位': unit }) df = pd.DataFrame(csv_data) csv_path = os.path.join(output_dir, 'ssd_evaluation_results.csv') df.to_csv(csv_path, index=False, encoding='utf-8-sig') logger.info(f" CSV结果保存至: {csv_path}") # 11. 清理临时目录 try: shutil.rmtree(temp_dir, ignore_errors=True) except Exception as e: logger.warning(f"清理临时目录时出错: {e}") return evaluation_report def main(): """主函数:使用val集评估SSD模型""" # 解析命令行参数 parser = argparse.ArgumentParser(description='SSD模型评估') parser.add_argument('--dataset', type=str, default='dataset11.0', help='数据集路径') parser.add_argument('--classes', type=str, default='dataset11.0/classes.txt', help='类别文件路径') parser.add_argument('--confidence', type=float, default=0.5, help='置信度阈值') parser.add_argument('--nms_iou', type=float, default=0.3, help='NMS IoU阈值') parser.add_argument('--output_dir', type=str, default='ssd_evaluation_results_val', help='输出目录') parser.add_argument('--max_images', type=int, default=None, help='最大测试图像数') parser.add_argument('--model_path', type=str, default=None, help='模型权重路径') args = parser.parse_args() config = { 'yolo_dataset_path': args.dataset, 'classes_path': args.classes, 'confidence': args.confidence, 'nms_iou': args.nms_iou, 'output_dir': args.output_dir, 'max_images': args.max_images, 'model_path': args.model_path } # 设置日志 logger = setup_logging(config['output_dir']) # 打印配置 logger.info("=" * 60) logger.info("SSD模型评估配置") logger.info("=" * 60) for key, value in config.items(): logger.info(f"{key}: {value}") # 初始化SSD模型 logger.info("加载SSD模型...") try: # 根据您的SSD项目初始化方式调整 # 这里是一个示例,您需要根据实际情况修改 ssd_model = SSD( confidence=config['confidence'], nms_iou=config['nms_iou'], model_path=config['model_path'] ) except Exception as e: logger.error(f"加载SSD模型失败: {e}") # 尝试其他初始化方式 try: # 尝试不同的参数 ssd_model = SSD() logger.info("使用默认参数初始化SSD模型") except Exception as e2: logger.error(f"默认初始化也失败: {e2}") return # 评估模型 logger.info("开始评估...") results = evaluate_ssd_val( model=ssd_model, classes_path=config['classes_path'], yolo_data_path=config['yolo_dataset_path'], output_dir=config['output_dir'], confidence=config['confidence'], nms_iou=config['nms_iou'], max_images=config['max_images'], logger=logger ) if results: logger.info("=" * 60) logger.info("SSD模型评估完成!") logger.info("=" * 60) # 打印关键指标 print("\n" + "=" * 60) print("SSD关键指标汇总:") print("=" * 60) print(f"1. 模型参数:") print(f" 参数量: {results['model_info']['total_parameters']:,} Parameters") print(f"\n2. 检测性能:") print(f" mAP50: {results['performance_metrics']['mAP50']:.4f}") print(f"\n3. 推理速度:") print(f" FPS: {results['inference_speed']['fps']:.2f}") print(f" 推理时间: {results['inference_speed']['avg_inference_time_ms']:.2f}ms") print(f"\n4. 数据集:") print(f" val集大小: {results['dataset_info']['val_set_size']}张") print(f" 实际测试: {results['dataset_info']['actual_tested_count']}张") print(f" 类别: {', '.join(results['dataset_info']['class_names'])}") print(f"\n详细报告已保存至: {config['output_dir']}") # 生成简洁报告 summary_path = os.path.join(config['output_dir'], 'SSD评估报告.txt') with open(summary_path, 'w', encoding='utf-8') as f: f.write("=" * 60 + "\n") f.write("SSD模型评估报告\n") f.write("=" * 60 + "\n\n") f.write(f"评估时间: {time.strftime('%Y-%m-%d %H:%M:%S')}\n\n") f.write("一、模型信息:\n") f.write(f" 总参数量: {results['model_info']['total_parameters']:,} Parameters\n") f.write(f" 置信度阈值: {results['model_info']['confidence_threshold']}\n") f.write(f" NMS IoU阈值: {results['model_info']['nms_iou_threshold']}\n\n") f.write("二、检测性能:\n") f.write(f" mAP50: {results['performance_metrics']['mAP50']:.4f}\n\n") f.write("三、推理速度:\n") f.write(f" FPS: {results['inference_speed']['fps']:.2f}\n") f.write(f" 平均推理时间: {results['inference_speed']['avg_inference_time_ms']:.2f}ms\n") f.write(f" 测试图像数量: {results['inference_speed']['test_images_count']}张\n\n") f.write("四、数据集信息:\n") f.write(f" val集大小: {results['dataset_info']['val_set_size']}张\n") f.write(f" 实际测试: {results['dataset_info']['actual_tested_count']}张\n") f.write(f" 类别数量: {results['dataset_info']['num_classes']}\n") f.write(f" 类别列表: {', '.join(results['dataset_info']['class_names'])}\n") print(f"简洁报告: {summary_path}") else: logger.error("SSD评估失败!") if __name__ == "__main__": main() 这是我的ssd.py代码, import colorsys import os import time import warnings import numpy as np import torch import torch.backends.cudnn as cudnn from PIL import Image, ImageDraw, ImageFont from nets.ssd import SSD300 from utils.anchors import get_anchors from utils.utils import (cvtColor, get_classes, preprocess_input, resize_image, show_config) from utils.utils_bbox import BBoxUtility warnings.filterwarnings("ignore") #--------------------------------------------# # 使用自己训练好的模型预测需要修改3个参数 # model_path、backbone和classes_path都需要修改! # 如果出现shape不匹配 # 一定要注意训练时的config里面的num_classes、 # model_path和classes_path参数的修改 #--------------------------------------------# class SSD(object): _defaults = { #--------------------------------------------------------------------------# # 使用自己训练好的模型进行预测一定要修改model_path和classes_path! # model_path指向logs文件夹下的权值文件,classes_path指向model_data下的txt # # 训练好后logs文件夹下存在多个权值文件,选择验证集损失较低的即可。 # 验证集损失较低不代表mAP较高,仅代表该权值在验证集上泛化性能较好。 # 如果出现shape不匹配,同时要注意训练时的model_path和classes_path参数的修改 #--------------------------------------------------------------------------# "model_path" : 'logs/ep200-loss1.828-val_loss1.712.pth', "classes_path" : 'dataset11.0/classes.txt', #---------------------------------------------------------------------# # 用于预测的图像大小,和train时使用同一个即可 #---------------------------------------------------------------------# "input_shape" : [640, 640], #-------------------------------# # 主干网络的选择 # vgg或者mobilenetv2或者resnet50 #-------------------------------# "backbone" : "vgg", #---------------------------------------------------------------------# # 只有得分大于置信度的预测框会被保留下来 #---------------------------------------------------------------------# "confidence" : 0.5, #---------------------------------------------------------------------# # 非极大抑制所用到的nms_iou大小 #---------------------------------------------------------------------# "nms_iou" : 0.3, #---------------------------------------------------------------------# # 用于指定先验框的大小 [21, 45, 99, 153, 207, 261, 315] [30, 60, 111, 162, 213, 264, 315] #---------------------------------------------------------------------# 'anchors_size' : [21, 45, 99, 153, 207, 261, 315], #---------------------------------------------------------------------# # 该变量用于控制是否使用letterbox_image对输入图像进行不失真的resize, # 在多次测试后,发现关闭letterbox_image直接resize的效果更好 #---------------------------------------------------------------------# "letterbox_image" : False, #-------------------------------# # 是否使用Cuda # 没有GPU可以设置成False #-------------------------------# "cuda" : True, } @classmethod def get_defaults(cls, n): if n in cls._defaults: return cls._defaults[n] else: return "Unrecognized attribute name '" + n + "'" #---------------------------------------------------# # 初始化ssd #---------------------------------------------------# def __init__(self, **kwargs): self.__dict__.update(self._defaults) for name, value in kwargs.items(): setattr(self, name, value) #---------------------------------------------------# # 计算总的类的数量 #---------------------------------------------------# self.class_names, self.num_classes = get_classes(self.classes_path) self.anchors = torch.from_numpy(get_anchors(self.input_shape, self.anchors_size, self.backbone)).type(torch.FloatTensor) if self.cuda: self.anchors = self.anchors.cuda() self.num_classes = self.num_classes + 1 #---------------------------------------------------# # 画框设置不同的颜色 #---------------------------------------------------# hsv_tuples = [(x / self.num_classes, 1., 1.) for x in range(self.num_classes)] self.colors = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv_tuples)) self.colors = list(map(lambda x: (int(x[0] * 255), int(x[1] * 255), int(x[2] * 255)), self.colors)) self.bbox_util = BBoxUtility(self.num_classes) self.generate() show_config(**self._defaults) #---------------------------------------------------# # 载入模型 #---------------------------------------------------# def generate(self, onnx=False): #-------------------------------# # 载入模型与权值 #-------------------------------# self.net = SSD300(self.num_classes, self.backbone) device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') self.net.load_state_dict(torch.load(self.model_path, map_location=device)) self.net = self.net.eval() print('{} model, anchors, and classes loaded.'.format(self.model_path)) if not onnx: if self.cuda: self.net = torch.nn.DataParallel(self.net) self.net = self.net.cuda() #---------------------------------------------------# # 检测图片 #---------------------------------------------------# def detect_image(self, image, crop = False, count = False): #---------------------------------------------------# # 计算输入图片的高和宽 #---------------------------------------------------# image_shape = np.array(np.shape(image)[0:2]) #---------------------------------------------------------# # 在这里将图像转换成RGB图像,防止灰度图在预测时报错。 # 代码仅仅支持RGB图像的预测,所有其它类型的图像都会转化成RGB #---------------------------------------------------------# image = cvtColor(image) #---------------------------------------------------------# # 给图像增加灰条,实现不失真的resize # 也可以直接resize进行识别 #---------------------------------------------------------# image_data = resize_image(image, (self.input_shape[1], self.input_shape[0]), self.letterbox_image) #---------------------------------------------------------# # 添加上batch_size维度,图片预处理,归一化。 #---------------------------------------------------------# image_data = np.expand_dims(np.transpose(preprocess_input(np.array(image_data, dtype='float32')), (2, 0, 1)), 0) with torch.no_grad(): #---------------------------------------------------# # 转化成torch的形式 #---------------------------------------------------# images = torch.from_numpy(image_data).type(torch.FloatTensor) if self.cuda: images = images.cuda() #---------------------------------------------------------# # 将图像输入网络当中进行预测! #---------------------------------------------------------# outputs = self.net(images) #-----------------------------------------------------------# # 将预测结果进行解码 #-----------------------------------------------------------# results = self.bbox_util.decode_box(outputs, self.anchors, image_shape, self.input_shape, self.letterbox_image, nms_iou = self.nms_iou, confidence = self.confidence) #--------------------------------------# # 如果没有检测到物体,则返回原图 #--------------------------------------# if len(results[0]) <= 0: return image top_label = np.array(results[0][:, 4], dtype = 'int32') top_conf = results[0][:, 5] top_boxes = results[0][:, :4] #---------------------------------------------------------# # 设置字体与边框厚度 #---------------------------------------------------------# font = ImageFont.truetype(font='model_data/simhei.ttf', size=np.floor(3e-2 * np.shape(image)[1] + 0.5).astype('int32')) thickness = max((np.shape(image)[0] + np.shape(image)[1]) // self.input_shape[0], 1) #---------------------------------------------------------# # 计数 #---------------------------------------------------------# if count: print("top_label:", top_label) classes_nums = np.zeros([self.num_classes]) for i in range(self.num_classes): num = np.sum(top_label == i) if num > 0: print(self.class_names[i], " : ", num) classes_nums[i] = num print("classes_nums:", classes_nums) #---------------------------------------------------------# # 是否进行目标的裁剪 #---------------------------------------------------------# if crop: for i, c in list(enumerate(top_boxes)): top, left, bottom, right = top_boxes[i] top = max(0, np.floor(top).astype('int32')) left = max(0, np.floor(left).astype('int32')) bottom = min(image.size[1], np.floor(bottom).astype('int32')) right = min(image.size[0], np.floor(right).astype('int32')) dir_save_path = "img_crop" if not os.path.exists(dir_save_path): os.makedirs(dir_save_path) crop_image = image.crop([left, top, right, bottom]) crop_image.save(os.path.join(dir_save_path, "crop_" + str(i) + ".png"), quality=95, subsampling=0) print("save crop_" + str(i) + ".png to " + dir_save_path) #---------------------------------------------------------# # 图像绘制 #---------------------------------------------------------# for i, c in list(enumerate(top_label)): predicted_class = self.class_names[int(c)] box = top_boxes[i] score = top_conf[i] top, left, bottom, right = box top = max(0, np.floor(top).astype('int32')) left = max(0, np.floor(left).astype('int32')) bottom = min(image.size[1], np.floor(bottom).astype('int32')) right = min(image.size[0], np.floor(right).astype('int32')) label = '{} {:.2f}'.format(predicted_class, score) draw = ImageDraw.Draw(image) label_size = draw.textsize(label, font) label = label.encode('utf-8') print(label, top, left, bottom, right) if top - label_size[1] >= 0: text_origin = np.array([left, top - label_size[1]]) else: text_origin = np.array([left, top + 1]) for i in range(thickness): draw.rectangle([left + i, top + i, right - i, bottom - i], outline=self.colors[c]) draw.rectangle([tuple(text_origin), tuple(text_origin + label_size)], fill=self.colors[c]) draw.text(text_origin, str(label,'UTF-8'), fill=(0, 0, 0), font=font) del draw return image def get_FPS(self, image, test_interval): #---------------------------------------------------# # 计算输入图片的高和宽 #---------------------------------------------------# image_shape = np.array(np.shape(image)[0:2]) #---------------------------------------------------------# # 在这里将图像转换成RGB图像,防止灰度图在预测时报错。 # 代码仅仅支持RGB图像的预测,所有其它类型的图像都会转化成RGB #---------------------------------------------------------# image = cvtColor(image) #---------------------------------------------------------# # 给图像增加灰条,实现不失真的resize # 也可以直接resize进行识别 #---------------------------------------------------------# image_data = resize_image(image, (self.input_shape[1], self.input_shape[0]), self.letterbox_image) #---------------------------------------------------------# # 添加上batch_size维度,图片预处理,归一化。 #---------------------------------------------------------# image_data = np.expand_dims(np.transpose(preprocess_input(np.array(image_data, dtype='float32')), (2, 0, 1)), 0) with torch.no_grad(): #---------------------------------------------------# # 转化成torch的形式 #---------------------------------------------------# images = torch.from_numpy(image_data).type(torch.FloatTensor) if self.cuda: images = images.cuda() #---------------------------------------------------------# # 将图像输入网络当中进行预测! #---------------------------------------------------------# outputs = self.net(images) #-----------------------------------------------------------# # 将预测结果进行解码 #-----------------------------------------------------------# results = self.bbox_util.decode_box(outputs, self.anchors, image_shape, self.input_shape, self.letterbox_image, nms_iou = self.nms_iou, confidence = self.confidence) t1 = time.time() for _ in range(test_interval): with torch.no_grad(): #---------------------------------------------------------# # 将图像输入网络当中进行预测! #---------------------------------------------------------# outputs = self.net(images) #-----------------------------------------------------------# # 将预测结果进行解码 #-----------------------------------------------------------# results = self.bbox_util.decode_box(outputs, self.anchors, image_shape, self.input_shape, self.letterbox_image, nms_iou = self.nms_iou, confidence = self.confidence) t2 = time.time() tact_time = (t2 - t1) / test_interval return tact_time def convert_to_onnx(self, simplify, model_path): import onnx self.generate(onnx=True) im = torch.zeros(1, 3, *self.input_shape).to('cpu') # image size(1, 3, 512, 512) BCHW input_layer_names = ["images"] output_layer_names = ["output"] # Export the model print(f'Starting export with onnx {onnx.__version__}.') torch.onnx.export(self.net, im, f = model_path, verbose = False, opset_version = 12, training = torch.onnx.TrainingMode.EVAL, do_constant_folding = True, input_names = input_layer_names, output_names = output_layer_names, dynamic_axes = None) # Checks model_onnx = onnx.load(model_path) # load onnx model onnx.checker.check_model(model_onnx) # check onnx model # Simplify onnx if simplify: import onnxsim print(f'Simplifying with onnx-simplifier {onnxsim.__version__}.') model_onnx, check = onnxsim.simplify( model_onnx, dynamic_input_shape=False, input_shapes=None) assert check, 'assert check failed' onnx.save(model_onnx, model_path) print('Onnx model save as {}'.format(model_path)) def get_map_txt(self, image_id, image, class_names, map_out_path): f = open(os.path.join(map_out_path, "detection-results/"+image_id+".txt"),"w") #---------------------------------------------------# # 计算输入图片的高和宽 #---------------------------------------------------# image_shape = np.array(np.shape(image)[0:2]) #---------------------------------------------------------# # 在这里将图像转换成RGB图像,防止灰度图在预测时报错。 # 代码仅仅支持RGB图像的预测,所有其它类型的图像都会转化成RGB #---------------------------------------------------------# image = cvtColor(image) #---------------------------------------------------------# # 给图像增加灰条,实现不失真的resize # 也可以直接resize进行识别 #---------------------------------------------------------# image_data = resize_image(image, (self.input_shape[1], self.input_shape[0]), self.letterbox_image) #---------------------------------------------------------# # 添加上batch_size维度,图片预处理,归一化。 #---------------------------------------------------------# image_data = np.expand_dims(np.transpose(preprocess_input(np.array(image_data, dtype='float32')), (2, 0, 1)), 0) with torch.no_grad(): #---------------------------------------------------# # 转化成torch的形式 #---------------------------------------------------# images = torch.from_numpy(image_data).type(torch.FloatTensor) if self.cuda: images = images.cuda() #---------------------------------------------------------# # 将图像输入网络当中进行预测! #---------------------------------------------------------# outputs = self.net(images) #-----------------------------------------------------------# # 将预测结果进行解码 #-----------------------------------------------------------# results = self.bbox_util.decode_box(outputs, self.anchors, image_shape, self.input_shape, self.letterbox_image, nms_iou = self.nms_iou, confidence = self.confidence) #--------------------------------------# # 如果没有检测到物体,则返回原图 #--------------------------------------# if len(results[0]) <= 0: return top_label = np.array(results[0][:, 4], dtype = 'int32') top_conf = results[0][:, 5] top_boxes = results[0][:, :4] for i, c in list(enumerate(top_label)): predicted_class = self.class_names[int(c)] box = top_boxes[i] score = str(top_conf[i]) top, left, bottom, right = box if predicted_class not in class_names: continue f.write("%s %s %s %s %s %s\n" % (predicted_class, score[:6], str(int(left)), str(int(top)), str(int(right)),str(int(bottom)))) f.close() return 这是我的get_map.py代码。根据我提供的这3段代码,生成一段可以直接使用的代码
最新发布
12-17
compiling main.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/main.c(48): error: #256: invalid redeclaration of type name "Cylinder" (declared at line 10 of "../Core/Inc/path_planner.h") } Cylinder; ../Core/Src/main.c(73): error: #101: "TASK_SIMPLE_EXIT" has already been declared in the current scope TASK_SIMPLE_EXIT, ../Core/Src/main.c(74): error: #101: "TASK_SNAKE_PATH" has already been declared in the current scope TASK_SNAKE_PATH, ../Core/Src/main.c(75): error: #101: "TASK_CIRCLE_TWO_COLORS" has already been declared in the current scope TASK_CIRCLE_TWO_COLORS, ../Core/Src/main.c(76): error: #101: "TASK_COLUMN_SWAP" has already been declared in the current scope TASK_COLUMN_SWAP, ../Core/Src/main.c(77): error: #101: "TASK_RANDOM_DETECT" has already been declared in the current scope TASK_RANDOM_DETECT, ../Core/Src/main.c(78): error: #101: "TASK_DETECT_AND_PATH" has already been declared in the current scope TASK_DETECT_AND_PATH, ../Core/Src/main.c(79): error: #101: "TASK_NONE" has already been declared in the current scope TASK_NONE ../Core/Src/main.c(80): error: #256: invalid redeclaration of type name "TaskMode" (declared at line 20 of "../Core/Inc/path_planner.h") } TaskMode; ../Core/Src/main.c(398): error: #20: identifier "M_PI" is undefined current_x += encoder_dist * cosf(fused_yaw * M_PI / 180.0f); ../Core/Src/main.c: 2 warnings, 10 errors compiling pid.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\pid.c: 2 warnings, 0 errors compiling usart.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/usart.c: 2 warnings, 0 errors compiling ultrasonic.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\ultrasonic.c: 2 warnings, 0 errors compiling encoder.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ..\Core\Src\encoder.c: 3 warnings, 0 errors compiling kalman_filter.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\kalman_filter.c: 2 warnings, 0 errors compiling motor.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\motor.c: 2 warnings, 0 errors compiling jy61p.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\jy61p.c(18): warning: #1-D: last line of file ends without a newline } ..\Core\Src\jy61p.c: 3 warnings, 0 errors compiling i2c.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/i2c.c: 2 warnings, 0 errors compiling tim.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/tim.c: 2 warnings, 0 errors compiling gpio.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/gpio.c: 2 warnings, 0 errors compiling motion.c... ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\motion.c(13): error: #20: identifier "Cylinder" is undefined extern Cylinder cylinders[MAX_CYLINDERS]; ..\Core\Src\motion.c(13): error: #20: identifier "MAX_CYLINDERS" is undefined extern Cylinder cylinders[MAX_CYLINDERS]; ..\Core\Src\motion.c: 3 warnings, 2 errors compiling stm32f1xx_it.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/stm32f1xx_it.c: 2 warnings, 0 errors compiling stm32f1xx_hal_msp.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ../Core/Src/stm32f1xx_hal_msp.c: 2 warnings, 0 errors compiling path_planner.c... ../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline #endif /* __ENCODER_H */ ../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline #endif /* __MOTION_H */ ..\Core\Src\path_planner.c(51): warning: #1-D: last line of file ends without a newline } ..\Core\Src\path_planner.c: 3 warnings, 0 errors "carL\carL.axf" - 12 Error(s), 34 Warning(s). Target not created. 以下是现有的代码 main.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" #include "i2c.h" #include "tim.h" #include "usart.h" #include "gpio.h" #include "jy61p.h" #include "pid.h" #include "kalman_filter.h" #include "motor.h" #include "encoder.h" #include "ultrasonic.h" #include "motion.h" #include "path_planner.h" #include <stdio.h> #include <string.h> #include <math.h> /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ // ??????? #define MAX_CYLINDERS 10 typedef struct { int x, y, r; char color; uint8_t visited; } Cylinder; Cylinder cylinders[MAX_CYLINDERS]; int cylinder_count = 0; uint8_t mission_running = 0; uint32_t start_time = 0; #define TASK_TIMEOUT_MS 10000 #define DETECT_TIMEOUT_MS 30000 uint32_t timeout = TASK_TIMEOUT_MS; char openmv_buffer[128]; uint8_t openmv_index = 0; uint8_t rx_byte; uint8_t openmv_data_ready = 0; uint8_t task_selected = 0; PID_Controller speed_pid; PID_Controller angle_pid; KalmanFilter kf; // ???? typedef enum { TASK_SIMPLE_EXIT, TASK_SNAKE_PATH, TASK_CIRCLE_TWO_COLORS, TASK_COLUMN_SWAP, TASK_RANDOM_DETECT, TASK_DETECT_AND_PATH, TASK_NONE } TaskMode; TaskMode current_task = TASK_SIMPLE_EXIT; // ????? typedef enum { TASK_IDLE, TASK_DETECTING, TASK_PATH_PLANNING, TASK_EXECUTING, TASK_CIRCLE_COLUMN, TASK_COMPLETED, TASK_FAILED } TaskState; TaskState current_state = TASK_IDLE; // ????? typedef enum { MOVE_IDLE, MOVE_FORWARD, MOVE_TURNING, MOVE_CIRCLE } MoveState; MoveState move_state = MOVE_IDLE; // ?????? float current_x = 0.0f; float current_y = 0.0f; float current_angle = 0.0f; // ????? PathPlanner planner; // ???? void Detect_Cylinders(void); void Plan_Path(TaskMode task); void ExecuteStep(void); void Reset_Cylinders(void); float Calculate_Distance(int x1, int y1, int x2, int y2); void Parse_OpenMV_Data(void); /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); /* USER CODE BEGIN PFP */ void StartMission(void); void ExecuteMission(TaskMode task); void SimpleExitPath(void); void SnakePath(void); void CircleTwoColors(void); void ColumnSwapPath(void); void RandomDetectPath(void); void AddCylinder(int x, int y, int r, char color); void BuildPath(char color); void AvoidObstacle(void); void Send_Debug_Info(void); void SensorFusion_Init(void); float SensorFusion_Update(float imu_yaw, float encoder_angle); void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart); /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == &huart1) { if (rx_byte == '\n' || openmv_index >= sizeof(openmv_buffer) - 1) { openmv_buffer[openmv_index] = '\0'; openmv_index = 0; openmv_data_ready = 1; } else { openmv_buffer[openmv_index++] = rx_byte; } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); } } void StartMission() { mission_running = 1; start_time = HAL_GetTick(); current_state = TASK_DETECTING; Reset_Cylinders(); } uint8_t IsMissionTimeout() { return (HAL_GetTick() - start_time) > timeout; } void AddCylinder(int x, int y, int r, char color) { if (cylinder_count < MAX_CYLINDERS) { cylinders[cylinder_count].x = x; cylinders[cylinder_count].y = y; cylinders[cylinder_count].r = r; cylinders[cylinder_count].color = color; cylinders[cylinder_count].visited = 0; cylinder_count++; } } void Reset_Cylinders() { for (int i = 0; i < cylinder_count; i++) { cylinders[i].visited = 0; } } float Calculate_Distance(int x1, int y1, int x2, int y2) { return sqrtf(powf(x2 - x1, 2) + powf(y2 - y1, 2)); } void Parse_OpenMV_Data(void) { char *token = strtok(openmv_buffer, ";"); cylinder_count = 0; while (token != NULL && cylinder_count < MAX_CYLINDERS) { int x, y, r; char color; if (sscanf(token, "%d,%d,%d,%c", &x, &y, &r, &color) == 4) { AddCylinder(x, y, r, color); } token = strtok(NULL, ";"); } } void Detect_Cylinders(void) { // ???? HAL_Delay(2000); AddCylinder(100, 200, 30, 'W'); AddCylinder(300, 150, 35, 'B'); printf("Detected %d cylinders.\r\n", cylinder_count); } void Plan_Path(TaskMode task) { // ?????? Path_Init(&planner); switch (task) { case TASK_SIMPLE_EXIT: Path_AddPoint(&planner, 0, 200, 0); Path_AddPoint(&planner, 300, 200, 1); break; case TASK_SNAKE_PATH: for (int i = 0; i < cylinder_count; i++) { Path_AddPoint(&planner, cylinders[i].x, cylinders[i].y, 1); } break; case TASK_CIRCLE_TWO_COLORS: Path_AddPoint(&planner, 100, 200, 2); Path_AddPoint(&planner, 300, 150, 2); break; } } void ExecuteStep(void) { static uint8_t step_index = 0; if (step_index < planner.count) { PathPoint *point = &planner.points[step_index]; if (point->type == 0) { Move_Forward(100); // ???? } else if (point->type == 1) { Turn_Right(90); // ?? } else { CircleColumn(step_index); // ?? } step_index++; } else { current_state = TASK_COMPLETED; } } void TaskMachine_Run() { switch (current_state) { case TASK_IDLE: if (mission_running) { current_state = TASK_DETECTING; } break; case TASK_DETECTING: if (openmv_data_ready) { Parse_OpenMV_Data(); openmv_data_ready = 0; current_state = TASK_PATH_PLANNING; } else if (IsMissionTimeout()) { current_state = TASK_FAILED; } break; case TASK_PATH_PLANNING: Plan_Path(current_task); current_state = TASK_EXECUTING; break; case TASK_EXECUTING: ExecuteStep(); AvoidObstacle(); // ???? break; case TASK_CIRCLE_COLUMN: CircleTwoColors(); current_state = TASK_COMPLETED; break; case TASK_COMPLETED: Motor_Stop(); mission_running = 0; current_state = TASK_IDLE; break; case TASK_FAILED: Motor_Stop(); printf("Mission Failed!\r\n"); mission_running = 0; current_state = TASK_IDLE; break; } if (IsMissionTimeout() && current_state != TASK_DETECTING) { current_state = TASK_FAILED; } } /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_I2C1_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM4_Init(); MX_USART1_UART_Init(); /* USER CODE BEGIN 2 */ HAL_UART_Receive_IT(&huart1, &rx_byte, 1); HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); JY61P_Init(&hi2c1); PID_Init(&speed_pid, 1.0f, 0.1f, 0.05f); PID_Init(&angle_pid, 2.0f, 0.0f, 0.1f); KalmanFilter_Init(&kf, 0.001f, 0.003f, 0.03f); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); AddCylinder(100, 200, 30, 'W'); AddCylinder(300, 150, 35, 'B'); /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) == GPIO_PIN_RESET) { HAL_Delay(20); if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) == GPIO_PIN_RESET) { StartMission(); while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) == GPIO_PIN_RESET); } } if (mission_running) { TaskMachine_Run(); } // ??????? float encoder_dist = Get_Total_Distance(); JY61P_Data imu_data; JY61P_Read_Attitude(&imu_data); float fused_yaw = KalmanFilter_Update(&kf, imu_data.yaw, imu_data.gz, 0.01f); // ?????? current_x += encoder_dist * cosf(fused_yaw * M_PI / 180.0f); current_y += encoder_dist * sinf(fused_yaw * M_PI / 180.0f); current_angle = fused_yaw; HAL_Delay(10); } /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ /* USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ /* USER CODE END 4 */ /** * @brief This function is executed in case of error occurrence. * @retval None */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ __disable_irq(); while (1) { } /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */ gpio.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file gpio.c * @brief This file provides code for the configuration * of all used GPIO pins. ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "gpio.h" /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ /*----------------------------------------------------------------------------*/ /* Configure GPIO */ /*----------------------------------------------------------------------------*/ /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /** Configure pins as * Analog * Input * Output * EVENT_OUT * EXTI */ void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /* Configure TB6612 Control Pins */ GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* Configure PWM Pins (PB0, PB1) */ GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* Configure Ultrasonic TRIG (PB2, PB4) */ GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_4; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* Configure Ultrasonic ECHO (PB3, PB5) */ GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* Configure Start Button (PB11) */ GPIO_InitStruct.Pin = GPIO_PIN_11; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } /* USER CODE BEGIN 2 */ /* USER CODE END 2 */ i2c.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file i2c.c * @brief This file provides code for the configuration * of the I2C instances. ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "i2c.h" /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ I2C_HandleTypeDef hi2c1; /* I2C1 init function */ void MX_I2C1_Init(void) { /* USER CODE BEGIN I2C1_Init 0 */ /* USER CODE END I2C1_Init 0 */ /* USER CODE BEGIN I2C1_Init 1 */ /* USER CODE END I2C1_Init 1 */ hi2c1.Instance = I2C1; hi2c1.Init.ClockSpeed = 400000; hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; hi2c1.Init.OwnAddress2 = 0; hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; if (HAL_I2C_Init(&hi2c1) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN I2C1_Init 2 */ /* USER CODE END I2C1_Init 2 */ } void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(i2cHandle->Instance==I2C1) { /* USER CODE BEGIN I2C1_MspInit 0 */ /* USER CODE END I2C1_MspInit 0 */ __HAL_RCC_GPIOB_CLK_ENABLE(); /**I2C1 GPIO Configuration PB8 ------> I2C1_SCL PB9 ------> I2C1_SDA */ GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); __HAL_AFIO_REMAP_I2C1_ENABLE(); /* I2C1 clock enable */ __HAL_RCC_I2C1_CLK_ENABLE(); /* USER CODE BEGIN I2C1_MspInit 1 */ /* USER CODE END I2C1_MspInit 1 */ } } void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle) { if(i2cHandle->Instance==I2C1) { /* USER CODE BEGIN I2C1_MspDeInit 0 */ /* USER CODE END I2C1_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_I2C1_CLK_DISABLE(); /**I2C1 GPIO Configuration PB8 ------> I2C1_SCL PB9 ------> I2C1_SDA */ HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8); HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9); /* USER CODE BEGIN I2C1_MspDeInit 1 */ /* USER CODE END I2C1_MspDeInit 1 */ } } /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ tim.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file tim.c * @brief This file provides code for the configuration * of the TIM instances. ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "tim.h" /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim4; /* TIM2 init function */ void MX_TIM2_Init(void) { /* USER CODE BEGIN TIM2_Init 0 */ /* USER CODE END TIM2_Init 0 */ TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_OC_InitTypeDef sConfigOC = {0}; /* USER CODE BEGIN TIM2_Init 1 */ /* USER CODE END TIM2_Init 1 */ htim2.Instance = TIM2; htim2.Init.Prescaler = 71; htim2.Init.CounterMode = TIM_COUNTERMODE_UP; htim2.Init.Period = 999; htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_PWM_Init(&htim2) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) { Error_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM2_Init 2 */ /* USER CODE END TIM2_Init 2 */ HAL_TIM_MspPostInit(&htim2); } /* TIM3 init function */ void MX_TIM3_Init(void) { /* USER CODE BEGIN TIM3_Init 0 */ /* USER CODE END TIM3_Init 0 */ TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; /* USER CODE BEGIN TIM3_Init 1 */ /* USER CODE END TIM3_Init 1 */ htim3.Instance = TIM3; htim3.Init.Prescaler = 0; htim3.Init.CounterMode = TIM_COUNTERMODE_UP; htim3.Init.Period = 65535; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; sConfig.EncoderMode = TIM_ENCODERMODE_TI1; sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC1Prescaler = TIM_ICPSC_DIV1; sConfig.IC1Filter = 0; sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM3_Init 2 */ /* USER CODE END TIM3_Init 2 */ } /* TIM4 init function */ void MX_TIM4_Init(void) { /* USER CODE BEGIN TIM4_Init 0 */ /* USER CODE END TIM4_Init 0 */ TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; /* USER CODE BEGIN TIM4_Init 1 */ /* USER CODE END TIM4_Init 1 */ htim4.Instance = TIM4; htim4.Init.Prescaler = 0; htim4.Init.CounterMode = TIM_COUNTERMODE_UP; htim4.Init.Period = 65535; htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; sConfig.EncoderMode = TIM_ENCODERMODE_TI1; sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC1Prescaler = TIM_ICPSC_DIV1; sConfig.IC1Filter = 0; sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM4_Init 2 */ /* USER CODE END TIM4_Init 2 */ } void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle) { if(tim_pwmHandle->Instance==TIM2) { /* USER CODE BEGIN TIM2_MspInit 0 */ /* USER CODE END TIM2_MspInit 0 */ /* TIM2 clock enable */ __HAL_RCC_TIM2_CLK_ENABLE(); /* USER CODE BEGIN TIM2_MspInit 1 */ /* USER CODE END TIM2_MspInit 1 */ } } void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(tim_encoderHandle->Instance==TIM3) { /* USER CODE BEGIN TIM3_MspInit 0 */ /* USER CODE END TIM3_MspInit 0 */ /* TIM3 clock enable */ __HAL_RCC_TIM3_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); /**TIM3 GPIO Configuration PA6 ------> TIM3_CH1 PA7 ------> TIM3_CH2 */ GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* TIM3 interrupt Init */ HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM3_IRQn); /* USER CODE BEGIN TIM3_MspInit 1 */ /* USER CODE END TIM3_MspInit 1 */ } else if(tim_encoderHandle->Instance==TIM4) { /* USER CODE BEGIN TIM4_MspInit 0 */ /* USER CODE END TIM4_MspInit 0 */ /* TIM4 clock enable */ __HAL_RCC_TIM4_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /**TIM4 GPIO Configuration PB6 ------> TIM4_CH1 PB7 ------> TIM4_CH2 */ GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* TIM4 interrupt Init */ HAL_NVIC_SetPriority(TIM4_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM4_IRQn); /* USER CODE BEGIN TIM4_MspInit 1 */ /* USER CODE END TIM4_MspInit 1 */ } } void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(timHandle->Instance==TIM2) { /* USER CODE BEGIN TIM2_MspPostInit 0 */ /* USER CODE END TIM2_MspPostInit 0 */ __HAL_RCC_GPIOA_CLK_ENABLE(); /**TIM2 GPIO Configuration PA0-WKUP ------> TIM2_CH1 PA1 ------> TIM2_CH2 */ GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* USER CODE BEGIN TIM2_MspPostInit 1 */ /* USER CODE END TIM2_MspPostInit 1 */ } } void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle) { if(tim_pwmHandle->Instance==TIM2) { /* USER CODE BEGIN TIM2_MspDeInit 0 */ /* USER CODE END TIM2_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_TIM2_CLK_DISABLE(); /* USER CODE BEGIN TIM2_MspDeInit 1 */ /* USER CODE END TIM2_MspDeInit 1 */ } } void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle) { if(tim_encoderHandle->Instance==TIM3) { /* USER CODE BEGIN TIM3_MspDeInit 0 */ /* USER CODE END TIM3_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_TIM3_CLK_DISABLE(); /**TIM3 GPIO Configuration PA6 ------> TIM3_CH1 PA7 ------> TIM3_CH2 */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_6|GPIO_PIN_7); /* TIM3 interrupt Deinit */ HAL_NVIC_DisableIRQ(TIM3_IRQn); /* USER CODE BEGIN TIM3_MspDeInit 1 */ /* USER CODE END TIM3_MspDeInit 1 */ } else if(tim_encoderHandle->Instance==TIM4) { /* USER CODE BEGIN TIM4_MspDeInit 0 */ /* USER CODE END TIM4_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_TIM4_CLK_DISABLE(); /**TIM4 GPIO Configuration PB6 ------> TIM4_CH1 PB7 ------> TIM4_CH2 */ HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6|GPIO_PIN_7); /* TIM4 interrupt Deinit */ HAL_NVIC_DisableIRQ(TIM4_IRQn); /* USER CODE BEGIN TIM4_MspDeInit 1 */ /* USER CODE END TIM4_MspDeInit 1 */ } } /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ usart.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file usart.c * @brief This file provides code for the configuration * of the USART instances. ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "usart.h" #include "stdio.h" #include "string.h" /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ UART_HandleTypeDef huart1; /* USART1 init function */ void MX_USART1_UART_Init(void) { /* USER CODE BEGIN USART1_Init 0 */ /* USER CODE END USART1_Init 0 */ /* USER CODE BEGIN USART1_Init 1 */ /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; huart1.Init.BaudRate = 115200; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; huart1.Init.Mode = UART_MODE_TX_RX; huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart1.Init.OverSampling = UART_OVERSAMPLING_16; if (HAL_UART_Init(&huart1) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN USART1_Init 2 */ /* USER CODE END USART1_Init 2 */ } void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(uartHandle->Instance==USART1) { /* USER CODE BEGIN USART1_MspInit 0 */ /* USER CODE END USART1_MspInit 0 */ /* USART1 clock enable */ __HAL_RCC_USART1_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX */ GPIO_InitStruct.Pin = GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* USART1 interrupt Init */ HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspInit 1 */ /* USER CODE END USART1_MspInit 1 */ } } void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) { if(uartHandle->Instance==USART1) { /* USER CODE BEGIN USART1_MspDeInit 0 */ /* USER CODE END USART1_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_USART1_CLK_DISABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); /* USART1 interrupt Deinit */ HAL_NVIC_DisableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspDeInit 1 */ /* USER CODE END USART1_MspDeInit 1 */ } } /* USER CODE BEGIN 1 */ void Send_Debug_Info(void) { char buffer[100]; sprintf(buffer, "Left: %.2f cm, Right: %.2f cm\r\n", Get_Left_Distance(), Get_Right_Distance()); HAL_UART_Transmit(&huart1, (uint8_t*)buffer, strlen(buffer), HAL_MAX_DELAY); } /* USER CODE END 1 */ motion.c #include "motion.h" #include "motor.h" #include "encoder.h" #include "jy61p.h" #include "pid.h" #include "stdio.h" #include <math.h> #ifndef M_PI #define M_PI 3.14159265358979323846 #endif extern Cylinder cylinders[MAX_CYLINDERS]; extern int cylinder_count; extern PID_Controller angle_pid; float current_x = 0.0f; float current_y = 0.0f; float current_angle = 0.0f; void Move_Forward(float distance) { float start_ticks = Get_Total_Distance(); float target_ticks = start_ticks + distance * 10.0f; Motor_Forward(80); while (Get_Total_Distance() < target_ticks) { HAL_Delay(10); } Motor_Stop(); } void Move_Backward(float distance) { float start_ticks = Get_Total_Distance(); float target_ticks = start_ticks + distance * 10.0f; Motor_Backward(80); while (Get_Total_Distance() < target_ticks) { HAL_Delay(10); } Motor_Stop(); } void Turn_Left(float angle) { JY61P_Data imu; JY61P_Read_Attitude(&imu); float target_yaw = imu.yaw - angle; if (target_yaw < -180.0f) target_yaw += 360.0f; Motor_Turn_Left(40); do { JY61P_Read_Attitude(&imu); HAL_Delay(10); } while (fabsf(imu.yaw - target_yaw) > 5.0f); Motor_Stop(); current_angle = target_yaw; } void Turn_Right(float angle) { JY61P_Data imu; JY61P_Read_Attitude(&imu); float target_yaw = imu.yaw + angle; if (target_yaw > 180.0f) target_yaw -= 360.0f; Motor_Turn_Right(40); do { JY61P_Read_Attitude(&imu); HAL_Delay(10); } while (fabsf(imu.yaw - target_yaw) > 5.0f); Motor_Stop(); current_angle = target_yaw; } void CircleColumn(int index) { float center_x = cylinders[index].x; float center_y = cylinders[index].y; float radius = cylinders[index].r + 15.0f; for (int i = 0; i < 36; i++) { float angle = current_angle + i * 10 * M_PI / 180.0f; float target_x = center_x + radius * cosf(angle); float target_y = center_y + radius * sinf(angle); float dist = sqrtf(powf(target_x - current_x, 2) + powf(target_y - current_y, 2)); float target_angle = atan2f(target_y - current_y, target_x - current_x) * 180.0f / M_PI; Turn_Right(target_angle - current_angle); Move_Forward(dist); } cylinders[index].visited = 1; } void Follow_Wall(float distance) { float wall_dist = Get_Left_Distance(); float start_x = current_x; float start_y = current_y; Motor_Forward(70); while (sqrtf(powf(current_x - start_x, 2) + powf(current_y - start_y, 2)) < distance) { float error = Get_Left_Distance() - wall_dist; float steer = PID_Update(&angle_pid, error, 0.01f); int left_speed = 70 + steer; int right_speed = 70 - steer; Motor_SetSpeed(left_speed, right_speed); HAL_Delay(10); } Motor_Stop(); } void AvoidObstacle() { float left = Get_Left_Distance(); float right = Get_Right_Distance(); if (left < 15 || right < 15) { Motor_Backward(10); if (left < right) { Turn_Right(60); } else { Turn_Left(60); } } } path_planner.c #include "path_planner.h" void Path_Init(PathPlanner *planner) { planner->count = 0; } void Path_AddPoint(PathPlanner *planner, int x, int y, uint8_t type) { if (planner->count < MAX_PATH_POINTS) { planner->points[planner->count].x = x; planner->points[planner->count].y = y; planner->points[planner->count].type = type; planner->count++; } } void Path_Generate(PathPlanner *planner, Cylinder *cylinders, uint8_t count, TaskMode task) { switch (task) { case TASK_SIMPLE_EXIT: Path_AddPoint(planner, 0, 200, 0); Path_AddPoint(planner, 300, 200, 1); break; case TASK_SNAKE_PATH: for (int i = 0; i < count; i++) { Path_AddPoint(planner, cylinders[i].x, cylinders[i].y, 1); } break; case TASK_CIRCLE_TWO_COLORS: Path_AddPoint(planner, 100, 200, 2); Path_AddPoint(planner, 300, 150, 2); break; } } uint8_t Path_Execute(PathPlanner *planner) { static uint8_t step_index = 0; if (step_index < planner->count) { PathPoint *point = &planner->points[step_index]; if (point->type == 0) { Move_Forward(100); } else if (point->type == 1) { Turn_Right(90); } else { CircleColumn(step_index); } step_index++; return 0; } else { step_index = 0; return 1; } } pid.c #include "pid.h" void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd) { pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->integral = 0.0f; pid->last_error = 0.0f; } float PID_Update(PID_Controller *pid, float error, float dt) { pid->integral += error * dt; float derivative = (error - pid->last_error) / dt; float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative; pid->last_error = error; return output; } ultrasonic.c #include "ultrasonic.h" float Get_Left_Distance(void) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_SET); HAL_Delay(10); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_RESET); uint32_t start = 0, end = 0; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_RESET); start = htim3.Instance->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET); end = htim3.Instance->CNT; return (end - start) * 0.034 / 2; } float Get_Right_Distance(void) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET); HAL_Delay(10); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); uint32_t start = 0, end = 0; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_RESET); start = htim3.Instance->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET); end = htim3.Instance->CNT; return (end - start) * 0.034 / 2; } kalman_filter.c #include "kalman_filter.h" void KalmanFilter_Init(KalmanFilter *kf, float Q_angle, float Q_bias, float R_measure) { kf->Q_angle = Q_angle; kf->Q_bias = Q_bias; kf->R_measure = R_measure; kf->angle = 0.0f; kf->bias = 0.0f; kf->P[0][0] = 0.0f; kf->P[0][1] = 0.0f; kf->P[1][0] = 0.0f; kf->P[1][1] = 0.0f; } float KalmanFilter_Update(KalmanFilter *kf, float new_angle, float new_rate, float dt) { kf->rate = new_rate - kf->bias; kf->angle += dt * kf->rate; kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= dt * kf->P[1][1]; kf->P[1][0] -= dt * kf->P[1][1]; kf->P[1][1] += kf->Q_bias * dt; float y = new_angle - kf->angle; float S = kf->P[0][0] + kf->R_measure; float K0 = kf->P[0][0] / S; float K1 = kf->P[1][0] / S; kf->angle += K0 * y; kf->bias += K1 * y; float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K0 * P00_temp; kf->P[0][1] -= K0 * P01_temp; kf->P[1][0] -= K1 * P00_temp; kf->P[1][1] -= K1 * P01_temp; return kf->angle; } motor.c #include "motor.h" void Motor_SetSpeed(int left, int right) { __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, left); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, right); } void Motor_Forward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); Motor_SetSpeed(speed, speed); } void Motor_Backward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); Motor_SetSpeed(speed, speed); } void Motor_Stop() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); Motor_SetSpeed(0, 0); } void Motor_Turn_Left(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); Motor_SetSpeed(speed, speed); } void Motor_Turn_Right(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); Motor_SetSpeed(speed, speed); } encoder.c #include "encoder.h" void Reset_Encoders(void) { htim3.Instance->CNT = 0; htim4.Instance->CNT = 0; } float Get_Total_Distance(void) { float left = htim3.Instance->CNT * 0.1f; float right = htim4.Instance->CNT * 0.1f; return (left + right) / 2.0f; } float Get_Left_Encoder(void) { return htim3.Instance->CNT * 0.1f; } float Get_Right_Encoder(void) { return htim4.Instance->CNT * 0.1f; } jy61p.c #include "jy61p.h" #include "main.h" extern I2C_HandleTypeDef hi2c1; #define JY61P_ADDR 0x50 << 1 void JY61P_Init(I2C_HandleTypeDef *hi2c) { // ³õʼ»¯ } void JY61P_Read_Attitude(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x56, 1, buf, 6, HAL_MAX_DELAY); data->pitch = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 180; data->roll = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 180; data->yaw = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 180; }
08-01
main.c /* USER CODE BEGIN Header / /* @file : main.c @brief : Main program body @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header / / Includes ------------------------------------------------------------------/ #include “main.h” #include “i2c.h” #include “tim.h” #include “usart.h” #include “gpio.h” #include “jy61p.h” #include “pid.h” #include “kalman_filter.h” #include “motor.h” #include “encoder.h” #include “ultrasonic.h” #include “motion.h” #include “stdio.h” #include “string.h” / Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes */ // Ô²ÖùÐÅÏ¢½á¹¹Ìå #define MAX_CYLINDERS 10 typedef struct { int x, y, r; char color; } Cylinder; Cylinder cylinders[MAX_CYLINDERS]; int cylinder_count = 0; uint8_t mission_running = 0; uint32_t start_time = 0; uint32_t timeout = 10000; // 10s uint8_t rx_byte; char openmv_buffer[128]; uint8_t openmv_index = 0; PID_Controller speed_pid; PID_Controller angle_pid; KalmanFilter kf; // ÈÎÎñģʽö¾Ù typedef enum { TASK_SIMPLE_EXIT, TASK_SNAKE_PATH, TASK_CIRCLE_TWO_COLORS, TASK_COLUMN_SWAP, TASK_RANDOM_DETECT } TaskMode; TaskMode current_task = TASK_SIMPLE_EXIT; // ״̬»ú¶¨Òå typedef enum { TASK_IDLE, TASK_MOVING_FORWARD, TASK_TURNING, TASK_AVOIDING, TASK_COMPLETED } TaskState; TaskState current_state = TASK_IDLE; /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------/ / USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------/ / USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------/ / USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------/ void SystemClock_Config(void); / USER CODE BEGIN PFP */ void StartMission(void); void ExecuteMission(TaskMode task); void SimpleExitPath(void); void SnakePath(void); void CircleTwoColors(void); void ColumnSwapPath(void); void RandomDetectPath(void); void AddCylinder(int x, int y, int r, char color); void BuildPath(char color); void AvoidObstacle(void); void Send_Debug_Info(void); void SensorFusion_Init(void); float SensorFusion_Update(float imu_yaw, float encoder_angle); void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart); /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------/ / USER CODE BEGIN 0 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == &huart1) { if (rx_byte >= ‘1’ && rx_byte <= ‘5’) { current_task = rx_byte - ‘1’; // 1~5 Ó³É䵽ö¾Ù printf(“Selected Task: %d\r\n”, current_task + 1); } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //¼ÌÐø½ÓÊÕ } } // Æô¶¯ÈÎÎñ void StartMission() { mission_running = 1; start_time = HAL_GetTick(); current_state = TASK_IDLE; } // ÊÇ·ñ³¬Ê± uint8_t IsMissionTimeout() { return (HAL_GetTick() - start_time) > timeout; } // Ìí¼ÓÔ²Öùµ½µØÍ¼ void AddCylinder(int x, int y, int r, char color) { if (cylinder_count < MAX_CYLINDERS) { cylinders[cylinder_count].x = x; cylinders[cylinder_count].y = y; cylinders[cylinder_count].r = r; cylinders[cylinder_count].color = color; cylinder_count++; } } // ¹¹½¨ÈÆÏß·¾¶ void BuildPath(char color) { if (color == ‘W’) { Turn_Left(90); } else if (color == ‘B’) { Turn_Right(90); } } //±ÜÕÏ void AvoidObstacle() { float left = Get_Left_Distance(); float right = Get_Right_Distance(); if (left < 10 && right < 10) { Motor_Stop(); printf(“Obstacle detected in both directions. Stopping.\r\n”); } else if (left < 10) { Turn_Right(90); } else if (right < 10) { Turn_Left(90); } } //¼òµ¥Â·¾¶ void SimpleExitPath() { Move_Forward(100); Turn_Right(90); Move_Forward(50); } //ÉßÐη¾¶ void SnakePath() { for (int i = 0; i < cylinder_count; i++) { if (i % 2 == 0) { Turn_Left(90); } else { Turn_Right(90); } Move_Forward(20); } } //ÈÆÁ½¸öÑÕɫԲÖù void CircleTwoColors() { char colors_seen[2] = {0}; int count = 0; for (int i = 0; i < cylinder_count && count < 2; i++) { if (colors_seen[0] == 0 && cylinders[i].color == ‘W’) { BuildPath(‘W’); colors_seen[0] = ‘W’; count++; } else if (colors_seen[1] == 0 && cylinders[i].color == ‘B’) { BuildPath(‘B’); colors_seen[1] = ‘B’; count++; } } } //ºÚ°×Öù½»»»ºó·¾¶ void ColumnSwapPath() { //¼ÙÉèÒѽ»»»Öù×ÓÖØÐ¹滮·¾¶ BuildPath(‘W’); BuildPath(‘B’); } // Ëæ»ú̽²â·¾¶ void RandomDetectPath() { // ½ÓÊÕOpenMV Êý¾Ý HAL_UART_Receive_IT(&huart1, &rx_byte, 1); HAL_Delay(30000); // 30s BuildPath(‘W’); // ¼ÙÉè̽²âµ½°×Öù Move_Forward(100); } void ExecuteMission(TaskMode task); void ExecuteMission(TaskMode task) { switch (task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; default: printf(“Unknown task mode: %d\r\n”, task); break; } } // ״̬»ú¿ØÖÆÈÎÎñ void TaskMachine_Run() { switch (current_state) { case TASK_IDLE: if (mission_running) { current_state = TASK_MOVING_FORWARD; } break; case TASK_MOVING_FORWARD: switch (current_task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; } current_state = TASK_COMPLETED; break; case TASK_COMPLETED: Motor_Stop(); mission_running = 0; current_state = TASK_IDLE; break; } } /* USER CODE END 0 */ /** @brief The application entry point. @retval int / int main(void) { / USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals / MX_GPIO_Init(); MX_I2C1_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM4_Init(); MX_USART1_UART_Init(); / USER CODE BEGIN 2 / HAL_UART_Receive_IT(&huart1, &rx_byte, 1); StartMission(); //Æô¶¯±àÂëÆ÷¼ÆÊý HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); // ³õʼ»¯ IMU JY61P_Init(&hi2c1); // ³õʼ»¯ PID PID_Init(&speed_pid, 1.0f, 0.1f, 0.05f); PID_Init(&angle_pid, 2.0f, 0.0f, 0.1f); // ³õʼ»¯ ¿¨¶ûÂüÂ˲¨ KalmanFilter_Init(&kf, 0.001f, 0.003f, 0.03f); // Æô¶¯ PWM Êä³ö HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); //Ìí¼Ó²âÊÔÔ²Öù AddCylinder(100, 200, 30, ‘W’); AddCylinder(300, 150, 35, ‘B’); / USER CODE END 2 */ /* Infinite loop / / USER CODE BEGIN WHILE */ while (1) { if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { HAL_Delay(20); if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { StartMission(); while (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET); // µÈ´ýÊÍ·Å } } if (mission_running) { ExecuteMission(current_task); //Ö´Ðе±Ç°ÈÎÎñ if (IsMissionTimeout()) { mission_running = 0; Motor_Stop(); printf(“Mission Timeout!\r\n”); } /* USER CODE END WHILE */ /* USER CODE BEGIN 3 / / USER CODE END 3 */ } } } /** @brief System Clock Configuration @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Initializes the RCC Oscillators according to the specified parameters in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ /* USER CODE END 4 */ /** @brief This function is executed in case of error occurrence. @retval None / void Error_Handler(void) { / USER CODE BEGIN Error_Handler_Debug / / User can add his own implementation to report the HAL error return state / __disable_irq(); while (1) { } / USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** @brief Reports the name of the source file and the source line number where the assert_param error has occurred. @param file: pointer to the source file name @param line: assert_param error line source number @retval None */ void assert_failed(uint8_t file, uint32_t line) { / USER CODE BEGIN 6 / / User can add his own implementation to report the file name and line number, ex: printf(“Wrong parameters value: file %s on line %d\r\n”, file, line) / / USER CODE END 6 / } #endif / USE_FULL_ASSERT */ gpio.c / USER CODE BEGIN Header / / @file gpio.c @brief This file provides code for the configuration of all used GPIO pins. @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include “gpio.h” /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ /----------------------------------------------------------------------------/ /* Configure GPIO / /----------------------------------------------------------------------------/ / USER CODE BEGIN 1 */ /* USER CODE END 1 */ /** Configure pins as Analog Input Output EVENT_OUT EXTI */ void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2|GPIO_PIN_4, GPIO_PIN_RESET); /*Configure GPIO pins : PB2 PB4 */ GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_4 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /*Configure GPIO pins : PB3 PB5 */ GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } /* USER CODE BEGIN 2 */ /* USER CODE END 2 / ic2.c / USER CODE BEGIN Header / /* @file i2c.c @brief This file provides code for the configuration of the I2C instances. @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header / / Includes ------------------------------------------------------------------*/ #include “i2c.h” /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ I2C_HandleTypeDef hi2c1; /* I2C1 init function */ void MX_I2C1_Init(void) { /* USER CODE BEGIN I2C1_Init 0 */ /* USER CODE END I2C1_Init 0 */ /* USER CODE BEGIN I2C1_Init 1 */ /* USER CODE END I2C1_Init 1 / hi2c1.Instance = I2C1; hi2c1.Init.ClockSpeed = 400000; hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; hi2c1.Init.OwnAddress2 = 0; hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; if (HAL_I2C_Init(&hi2c1) != HAL_OK) { Error_Handler(); } / USER CODE BEGIN I2C1_Init 2 */ /* USER CODE END I2C1_Init 2 */ } void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(i2cHandle->Instance==I2C1) { /* USER CODE BEGIN I2C1_MspInit 0 */ /* USER CODE END I2C1_MspInit 0 */ __HAL_RCC_GPIOB_CLK_ENABLE(); /**I2C1 GPIO Configuration PB8 ------> I2C1_SCL PB9 ------> I2C1_SDA / GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); __HAL_AFIO_REMAP_I2C1_ENABLE(); / I2C1 clock enable / __HAL_RCC_I2C1_CLK_ENABLE(); / USER CODE BEGIN I2C1_MspInit 1 */ /* USER CODE END I2C1_MspInit 1 */ } } void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle) { if(i2cHandle->Instance==I2C1) { /* USER CODE BEGIN I2C1_MspDeInit 0 */ /* USER CODE END I2C1_MspDeInit 0 / / Peripheral clock disable */ __HAL_RCC_I2C1_CLK_DISABLE(); /**I2C1 GPIO Configuration PB8 ------> I2C1_SCL PB9 ------> I2C1_SDA / HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8); HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9); / USER CODE BEGIN I2C1_MspDeInit 1 */ /* USER CODE END I2C1_MspDeInit 1 */ } } /* USER CODE BEGIN 1 */ /* USER CODE END 1 / tim.c / USER CODE BEGIN Header / /* @file tim.c @brief This file provides code for the configuration of the TIM instances. @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header / / Includes ------------------------------------------------------------------*/ #include “tim.h” /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim4; /* TIM2 init function */ void MX_TIM2_Init(void) { /* USER CODE BEGIN TIM2_Init 0 */ /* USER CODE END TIM2_Init 0 */ TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_OC_InitTypeDef sConfigOC = {0}; /* USER CODE BEGIN TIM2_Init 1 */ /* USER CODE END TIM2_Init 1 / htim2.Instance = TIM2; htim2.Init.Prescaler = 71; htim2.Init.CounterMode = TIM_COUNTERMODE_UP; htim2.Init.Period = 999; htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_PWM_Init(&htim2) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) { Error_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); } / USER CODE BEGIN TIM2_Init 2 */ /* USER CODE END TIM2_Init 2 */ HAL_TIM_MspPostInit(&htim2); } /* TIM3 init function */ void MX_TIM3_Init(void) { /* USER CODE BEGIN TIM3_Init 0 */ /* USER CODE END TIM3_Init 0 */ TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; /* USER CODE BEGIN TIM3_Init 1 */ /* USER CODE END TIM3_Init 1 / htim3.Instance = TIM3; htim3.Init.Prescaler = 0; htim3.Init.CounterMode = TIM_COUNTERMODE_UP; htim3.Init.Period = 65535; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; sConfig.EncoderMode = TIM_ENCODERMODE_TI1; sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC1Prescaler = TIM_ICPSC_DIV1; sConfig.IC1Filter = 0; sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) { Error_Handler(); } / USER CODE BEGIN TIM3_Init 2 */ /* USER CODE END TIM3_Init 2 */ } /* TIM4 init function */ void MX_TIM4_Init(void) { /* USER CODE BEGIN TIM4_Init 0 */ /* USER CODE END TIM4_Init 0 */ TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; /* USER CODE BEGIN TIM4_Init 1 */ /* USER CODE END TIM4_Init 1 / htim4.Instance = TIM4; htim4.Init.Prescaler = 0; htim4.Init.CounterMode = TIM_COUNTERMODE_UP; htim4.Init.Period = 65535; htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; sConfig.EncoderMode = TIM_ENCODERMODE_TI1; sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC1Prescaler = TIM_ICPSC_DIV1; sConfig.IC1Filter = 0; sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) { Error_Handler(); } / USER CODE BEGIN TIM4_Init 2 */ /* USER CODE END TIM4_Init 2 */ } void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle) { if(tim_pwmHandle->Instance==TIM2) { /* USER CODE BEGIN TIM2_MspInit 0 */ /* USER CODE END TIM2_MspInit 0 / / TIM2 clock enable / __HAL_RCC_TIM2_CLK_ENABLE(); / USER CODE BEGIN TIM2_MspInit 1 */ /* USER CODE END TIM2_MspInit 1 */ } } void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(tim_encoderHandle->Instance==TIM3) { /* USER CODE BEGIN TIM3_MspInit 0 */ /* USER CODE END TIM3_MspInit 0 / / TIM3 clock enable */ __HAL_RCC_TIM3_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); /**TIM3 GPIO Configuration PA6 ------> TIM3_CH1 PA7 ------> TIM3_CH2 / GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); / TIM3 interrupt Init / HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM3_IRQn); / USER CODE BEGIN TIM3_MspInit 1 */ /* USER CODE END TIM3_MspInit 1 / } else if(tim_encoderHandle->Instance==TIM4) { / USER CODE BEGIN TIM4_MspInit 0 */ /* USER CODE END TIM4_MspInit 0 / / TIM4 clock enable */ __HAL_RCC_TIM4_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /**TIM4 GPIO Configuration PB6 ------> TIM4_CH1 PB7 ------> TIM4_CH2 / GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); / TIM4 interrupt Init / HAL_NVIC_SetPriority(TIM4_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM4_IRQn); / USER CODE BEGIN TIM4_MspInit 1 */ /* USER CODE END TIM4_MspInit 1 / } } void HAL_TIM_MspPostInit(TIM_HandleTypeDef timHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(timHandle->Instance==TIM2) { /* USER CODE BEGIN TIM2_MspPostInit 0 */ /* USER CODE END TIM2_MspPostInit 0 */ __HAL_RCC_GPIOA_CLK_ENABLE(); /**TIM2 GPIO Configuration PA0-WKUP ------> TIM2_CH1 PA1 ------> TIM2_CH2 / GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); / USER CODE BEGIN TIM2_MspPostInit 1 */ /* USER CODE END TIM2_MspPostInit 1 */ } } void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle) { if(tim_pwmHandle->Instance==TIM2) { /* USER CODE BEGIN TIM2_MspDeInit 0 */ /* USER CODE END TIM2_MspDeInit 0 / / Peripheral clock disable / __HAL_RCC_TIM2_CLK_DISABLE(); / USER CODE BEGIN TIM2_MspDeInit 1 */ /* USER CODE END TIM2_MspDeInit 1 */ } } void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle) { if(tim_encoderHandle->Instance==TIM3) { /* USER CODE BEGIN TIM3_MspDeInit 0 */ /* USER CODE END TIM3_MspDeInit 0 / / Peripheral clock disable */ __HAL_RCC_TIM3_CLK_DISABLE(); /**TIM3 GPIO Configuration PA6 ------> TIM3_CH1 PA7 ------> TIM3_CH2 / HAL_GPIO_DeInit(GPIOA, GPIO_PIN_6|GPIO_PIN_7); / TIM3 interrupt Deinit / HAL_NVIC_DisableIRQ(TIM3_IRQn); / USER CODE BEGIN TIM3_MspDeInit 1 */ /* USER CODE END TIM3_MspDeInit 1 / } else if(tim_encoderHandle->Instance==TIM4) { / USER CODE BEGIN TIM4_MspDeInit 0 */ /* USER CODE END TIM4_MspDeInit 0 / / Peripheral clock disable */ __HAL_RCC_TIM4_CLK_DISABLE(); /**TIM4 GPIO Configuration PB6 ------> TIM4_CH1 PB7 ------> TIM4_CH2 / HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6|GPIO_PIN_7); / TIM4 interrupt Deinit / HAL_NVIC_DisableIRQ(TIM4_IRQn); / USER CODE BEGIN TIM4_MspDeInit 1 */ /* USER CODE END TIM4_MspDeInit 1 */ } } /* USER CODE BEGIN 1 */ /* USER CODE END 1 / usart.c / USER CODE BEGIN Header / /* @file usart.c @brief This file provides code for the configuration of the USART instances. @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header / / Includes ------------------------------------------------------------------/ #include “usart.h” #include “stdio.h” #include “string.h” / USER CODE BEGIN 0 */ /* USER CODE END 0 */ UART_HandleTypeDef huart1; /* USART1 init function */ void MX_USART1_UART_Init(void) { /* USER CODE BEGIN USART1_Init 0 */ /* USER CODE END USART1_Init 0 */ /* USER CODE BEGIN USART1_Init 1 */ /* USER CODE END USART1_Init 1 / huart1.Instance = USART1; huart1.Init.BaudRate = 115200; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; huart1.Init.Mode = UART_MODE_TX_RX; huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart1.Init.OverSampling = UART_OVERSAMPLING_16; if (HAL_UART_Init(&huart1) != HAL_OK) { Error_Handler(); } / USER CODE BEGIN USART1_Init 2 */ /* USER CODE END USART1_Init 2 */ } void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(uartHandle->Instance==USART1) { /* USER CODE BEGIN USART1_MspInit 0 */ /* USER CODE END USART1_MspInit 0 / / USART1 clock enable */ __HAL_RCC_USART1_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX / GPIO_InitStruct.Pin = GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); / USART1 interrupt Init / HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(USART1_IRQn); / USER CODE BEGIN USART1_MspInit 1 */ /* USER CODE END USART1_MspInit 1 */ } } void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) { if(uartHandle->Instance==USART1) { /* USER CODE BEGIN USART1_MspDeInit 0 */ /* USER CODE END USART1_MspDeInit 0 / / Peripheral clock disable */ __HAL_RCC_USART1_CLK_DISABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX / HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); / USART1 interrupt Deinit / HAL_NVIC_DisableIRQ(USART1_IRQn); / USER CODE BEGIN USART1_MspDeInit 1 */ /* USER CODE END USART1_MspDeInit 1 */ } } /* USER CODE BEGIN 1 / void Send_Debug_Info(void) { char buffer[100]; sprintf(buffer, “Left: %.2f cm, Right: %.2f cm\r\n”, Get_Left_Distance(), Get_Right_Distance()); HAL_UART_Transmit(&huart1, (uint8_t)buffer, strlen(buffer), HAL_MAX_DELAY); } /* USER CODE END 1 / stm32f1xx_it.c / USER CODE BEGIN Header / /* @file stm32f1xx_it.c @brief Interrupt Service Routines. @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header */ /* Includes ------------------------------------------------------------------/ #include “main.h” #include “stm32f1xx_it.h” / Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes / / USER CODE END Includes */ /* Private typedef -----------------------------------------------------------/ / USER CODE BEGIN TD */ /* USER CODE END TD */ /* Private define ------------------------------------------------------------/ / USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------/ / USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------/ / USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------/ / USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------/ / USER CODE BEGIN 0 */ /* USER CODE END 0 */ /* External variables --------------------------------------------------------/ extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim4; extern UART_HandleTypeDef huart1; / USER CODE BEGIN EV */ /* USER CODE END EV */ // / Cortex-M3 Processor Interruption and Exception Handlers / // /** @brief This function handles Non maskable interrupt. / void NMI_Handler(void) { / USER CODE BEGIN NonMaskableInt_IRQn 0 */ /* USER CODE END NonMaskableInt_IRQn 0 / / USER CODE BEGIN NonMaskableInt_IRQn 1 / while (1) { } / USER CODE END NonMaskableInt_IRQn 1 */ } /** @brief This function handles Hard fault interrupt. / void HardFault_Handler(void) { / USER CODE BEGIN HardFault_IRQn 0 */ /* USER CODE END HardFault_IRQn 0 / while (1) { / USER CODE BEGIN W1_HardFault_IRQn 0 / / USER CODE END W1_HardFault_IRQn 0 */ } } /** @brief This function handles Memory management fault. / void MemManage_Handler(void) { / USER CODE BEGIN MemoryManagement_IRQn 0 */ /* USER CODE END MemoryManagement_IRQn 0 / while (1) { / USER CODE BEGIN W1_MemoryManagement_IRQn 0 / / USER CODE END W1_MemoryManagement_IRQn 0 */ } } /** @brief This function handles Prefetch fault, memory access fault. / void BusFault_Handler(void) { / USER CODE BEGIN BusFault_IRQn 0 */ /* USER CODE END BusFault_IRQn 0 / while (1) { / USER CODE BEGIN W1_BusFault_IRQn 0 / / USER CODE END W1_BusFault_IRQn 0 */ } } /** @brief This function handles Undefined instruction or illegal state. / void UsageFault_Handler(void) { / USER CODE BEGIN UsageFault_IRQn 0 */ /* USER CODE END UsageFault_IRQn 0 / while (1) { / USER CODE BEGIN W1_UsageFault_IRQn 0 / / USER CODE END W1_UsageFault_IRQn 0 */ } } /** @brief This function handles System service call via SWI instruction. / void SVC_Handler(void) { / USER CODE BEGIN SVCall_IRQn 0 */ /* USER CODE END SVCall_IRQn 0 / / USER CODE BEGIN SVCall_IRQn 1 */ /* USER CODE END SVCall_IRQn 1 */ } /** @brief This function handles Debug monitor. / void DebugMon_Handler(void) { / USER CODE BEGIN DebugMonitor_IRQn 0 */ /* USER CODE END DebugMonitor_IRQn 0 / / USER CODE BEGIN DebugMonitor_IRQn 1 */ /* USER CODE END DebugMonitor_IRQn 1 */ } /** @brief This function handles Pendable request for system service. / void PendSV_Handler(void) { / USER CODE BEGIN PendSV_IRQn 0 */ /* USER CODE END PendSV_IRQn 0 / / USER CODE BEGIN PendSV_IRQn 1 */ /* USER CODE END PendSV_IRQn 1 */ } /** @brief This function handles System tick timer. / void SysTick_Handler(void) { / USER CODE BEGIN SysTick_IRQn 0 */ /* USER CODE END SysTick_IRQn 0 / HAL_IncTick(); / USER CODE BEGIN SysTick_IRQn 1 */ /* USER CODE END SysTick_IRQn 1 */ } // / STM32F1xx Peripheral Interrupt Handlers / / Add here the Interrupt Handlers for the used peripherals. / / For the available peripheral interrupt handler names, / / please refer to the startup file (startup_stm32f1xx.s). / // /** @brief This function handles TIM3 global interrupt. / void TIM3_IRQHandler(void) { / USER CODE BEGIN TIM3_IRQn 0 */ /* USER CODE END TIM3_IRQn 0 / HAL_TIM_IRQHandler(&htim3); / USER CODE BEGIN TIM3_IRQn 1 */ /* USER CODE END TIM3_IRQn 1 */ } /** @brief This function handles TIM4 global interrupt. / void TIM4_IRQHandler(void) { / USER CODE BEGIN TIM4_IRQn 0 */ /* USER CODE END TIM4_IRQn 0 / HAL_TIM_IRQHandler(&htim4); / USER CODE BEGIN TIM4_IRQn 1 */ /* USER CODE END TIM4_IRQn 1 */ } /** @brief This function handles USART1 global interrupt. / void USART1_IRQHandler(void) { / USER CODE BEGIN USART1_IRQn 0 */ /* USER CODE END USART1_IRQn 0 / HAL_UART_IRQHandler(&huart1); / USER CODE BEGIN USART1_IRQn 1 */ /* USER CODE END USART1_IRQn 1 */ } /* USER CODE BEGIN 1 */ /* USER CODE END 1 / stm32f1xx_hal_msp.c / USER CODE BEGIN Header / /* @file stm32f1xx_hal_msp.c @brief This file provides code for the MSP Initialization and de-Initialization codes. @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header */ /* Includes ------------------------------------------------------------------/ #include “main.h” / USER CODE BEGIN Includes */ /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------/ / USER CODE BEGIN TD */ /* USER CODE END TD */ /* Private define ------------------------------------------------------------/ / USER CODE BEGIN Define */ /* USER CODE END Define */ /* Private macro -------------------------------------------------------------/ / USER CODE BEGIN Macro */ /* USER CODE END Macro */ /* Private variables ---------------------------------------------------------/ / USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------/ / USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* External functions --------------------------------------------------------/ / USER CODE BEGIN ExternalFunctions */ /* USER CODE END ExternalFunctions */ /* USER CODE BEGIN 0 */ /* USER CODE END 0 / /* Initializes the Global MSP. / void HAL_MspInit(void) { / USER CODE BEGIN MspInit 0 */ /* USER CODE END MspInit 0 */ __HAL_RCC_AFIO_CLK_ENABLE(); __HAL_RCC_PWR_CLK_ENABLE(); /* System interrupt init*/ /** NOJTAG: JTAG-DP Disabled and SW-DP Enabled */ __HAL_AFIO_REMAP_SWJ_NOJTAG(); /* USER CODE BEGIN MspInit 1 */ /* USER CODE END MspInit 1 */ } /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ motion.c #include “motion.h” #include “encoder.h” #include “motor.h” #include “jy61p.h” #include “math.h” #include “stddef.h” void Move_Forward(int distance) { int target_ticks = distance * 10; int current_ticks = 0; Reset_Encoders(); Motor_Forward(100); while (1) { int left = Get_Left_Encoder(); int right = Get_Right_Encoder(); current_ticks = (left + right) / 2; if (current_ticks >= target_ticks) { Motor_Stop(); break; } } } void Move_Backward(int distance) { int target_ticks = distance * 10; int current_ticks = 0; Reset_Encoders(); Motor_Backward(100); while (1) { int left = Get_Left_Encoder(); int right = Get_Right_Encoder(); current_ticks = (left + right) / 2; if (current_ticks >= target_ticks) { Motor_Stop(); break; } } } void Turn_Left(int angle) { float target_angle = angle; float current_yaw = 0.0f; JY61P_Data imu_data; while (fabsf(current_yaw) < target_angle) { Motor_Turn_Left(50); JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; } Motor_Stop(); } void Turn_Right(int angle) { float target_angle = angle; float current_yaw = 0.0f; JY61P_Data imu_data; while (fabsf(current_yaw) < target_angle) { Motor_Turn_Right(50); JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; } Motor_Stop(); } pid.c #include “pid.h” void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd) { pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->integral = 0.0f; pid->last_error = 0.0f; } float PID_Update(PID_Controller *pid, float error, float dt) { pid->integral += error * dt; float derivative = (error - pid->last_error) / dt; float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative; pid->last_error = error; return output; } ultrasonic.c #include “ultrasonic.h” #include “tim.h” #include “gpio.h” float Get_Left_Distance() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_SET); HAL_Delay(10); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_RESET); uint32_t start = 0, end = 0; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_RESET); start = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } float Get_Right_Distance() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET); HAL_Delay(10); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); uint32_t start = 0, end = 0; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_RESET); start = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } motor.c #include “motor.h” #include “tim.h” void Motor_SetSpeed(int left, int right) { __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, left); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, right); } void Motor_Forward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed); } void Motor_Backward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed); } void Motor_Stop() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 0); } void Motor_Turn_Left(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed); } void Motor_Turn_Right(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed); } kalman_filter.c #include “kalman_filter.h” void KalmanFilter_Init(KalmanFilter *kf, float Q_angle, float Q_bias, float R_measure) { kf->Q_angle = Q_angle; kf->Q_bias = Q_bias; kf->R_measure = R_measure; kf->angle = 0.0f; kf->bias = 0.0f; kf->P[0][0] = 0.0f; kf->P[0][1] = 0.0f; kf->P[1][0] = 0.0f; kf->P[1][1] = 0.0f; } float KalmanFilter_Update(KalmanFilter *kf, float new_angle, float new_rate, float dt) { kf->rate = new_rate - kf->bias; kf->angle += dt * kf->rate; kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= dt * kf->P[1][1]; kf->P[1][0] -= dt * kf->P[1][1]; kf->P[1][1] += kf->Q_bias * dt; float y = new_angle - kf->angle; float S = kf->P[0][0] + kf->R_measure; float K[2]; K[0] = kf->P[0][0] / S; K[1] = kf->P[1][0] / S; kf->angle += K[0] * y; kf->bias += K[1] * y; float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K[0] * P00_temp; kf->P[0][1] -= K[0] * P01_temp; kf->P[1][0] -= K[1] * P00_temp; kf->P[1][1] -= K[1] * P01_temp; return kf->angle; } encoder.c #include “encoder.h” #include “tim.h” void Reset_Encoders(void) { htim3.Instance->CNT = 0; htim4.Instance->CNT = 0; } float Get_Total_Distance(void) { int32_t left_ticks = htim3.Instance->CNT; int32_t right_ticks = htim4.Instance->CNT; float distance = (left_ticks + right_ticks) / 2.0f * 0.1f; return distance; } float Get_Left_Encoder(void) { return (float)htim3.Instance->CNT; } float Get_Right_Encoder(void) { return (float)htim4.Instance->CNT; } jy61p.c #include “jy61p.h” #include <string.h> #include “i2c.h” #define JY61P_ADDR 0x50 << 1 void JY61P_Init(I2C_HandleTypeDef *hi2c) { } void JY61P_Read_Acc(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x34, 1, buf, 6, HAL_MAX_DELAY); data->ax = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 16; data->ay = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 16; data->az = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 16; } void JY61P_Read_Gyro(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x38, 1, buf, 6, HAL_MAX_DELAY); data->gx = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 2000; data->gy = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 2000; data->gz = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 2000; } void JY61P_Read_Attitude(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x56, 1, buf, 6, HAL_MAX_DELAY); data->pitch = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 180; data->roll = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 180; data->yaw = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 180; } 在这里的代码上进行修改硬件为一个jy61p陀螺仪,两个带电机MG513编码器,一个f103c8t6,两个HC-SR04超声波,两个红外测距,一个TB6612FNG,一个openMV,i2c1_SDA和SCL接在PB8和9,HC-SR04左TRIG和ECHO接在PB2和3右TRIG和ECHO接在PB4和5,TB6612FNG的ADC接在PA6,PWMB接在PB0,BIN1-2接PB13-12,STBY接在3.3v,AIN1-2接在PB14-15,PWMA接在PB1,修改为可以更好的实现一、 任务 制作一辆自动避障小车,从测试场地边墙入口 A 驶入,按具体任务要求绕 过场地内的圆柱障碍,在规定时间内从出口 C 驶出。测试场地如图 1 所示,为 边长 2 米的正方形,四周有边墙围挡,两侧有入口 A 和出口 C,4 个白色、5 个 黑色圆柱用圆柱座固定在场地上作为障碍物。二、 要求 小车在场地中行驶不应触碰圆柱,可触碰但不可跨越场地边墙。 基本要求 圆柱排列如图 1 所示。小车放置在准备区。 (1)一键启动小车并开始计时,小车从 A 口进入,任选路径,10s 内车身 完全从 C 口驶出。 (2)一键启动小车并开始计时,小车从 A 口进入,左右变向蛇行绕过第 2 行各圆柱,行驶轨迹参见说明(6),10s 内车身完全从 C 口驶出。 (3)一键启动小车并开始计时,小车从 A 口进入,分别绕任意两个不同颜 色的圆柱各转行 1 圈(方向不限),10s 内车身完全从 C 口驶出。 发挥部分 小车每次从入口到出口穿越过程中还必须满足:不从两个黑柱间穿过,沿边 墙墙角转弯不超过 1 次。 (1)小车放置在准备区,在图 1 的基础上按指令将第 1 行第 3 列处的黑柱 与任一白柱互换位置,一键启动小车并开始计时,从 A 口进入,计时至车身完 全从 C 口驶出,用时越少越好。 (2)小车放置在准备区,按指令随机排列圆柱,一键启动探测圆柱,在 30s 内完成探测、穿过出发线进入 A 口,10s 内车身应完全从 C 口驶出.三、 说明 (1)测试时场地、边墙及圆柱座由赛区提供。场地用长 300cm 宽 210cm 的 哑光喷绘布制作,背景色为灰色(R:170 G:170 B:170),喷绘出图 1 中圆 柱位置(直径 2.5cm 黑色圆圈)、黄色边墙位置、黑色出发线和黑色虚线,其线 宽分别为 0.2cm、1cm、2cm 和 0.2cm。障碍圆柱用直径 2cm 白色 PVC 电工穿线 管制作,长度 20±1cm。白柱采用 PVC 原色,黑柱表面涂哑光黑色。圆柱座采 用白色“直接套管”(PVC 管标准配件,长度 4~5cm)。在喷绘布上圆柱位置开 孔,将圆柱座用热熔胶粘接固定在地面上。场地边墙高 3±0.5cm,用木板制作, 表面黄色,用热熔胶固定。测试时参赛队必须自带圆柱,插入赛区准备好的圆柱 座。圆柱和圆柱座、场地及周边不得有任何其他标志和传感器。 (2)所有传感器及控制电路均安装在小车上,小车尺寸在任何状态均必须 满足(含所有传感器和控制电路):长≤35cm、宽≤25cm、高≤35cm。测试中小车 与外部不得有任何通信交互。 (3)小车上只有 1 个“启动按键”,设置好测试项目后可用其一键启动小车。 (4)发挥部分(2),小车一键启动后在 30s 探测时间内可在准备区中任意 移动。
08-01
main.c /* USER CODE BEGIN Header / /* @file : main.c @brief : Main program body @attention Copyright © 2025 STMicroelectronics. All rights reserved. This software is licensed under terms that can be found in the LICENSE file in the root directory of this software component. If no LICENSE file comes with this software, it is provided AS-IS. / / USER CODE END Header / / Includes ------------------------------------------------------------------/ #include “main.h” #include “i2c.h” #include “tim.h” #include “usart.h” #include “gpio.h” #include “jy61p.h” #include “pid.h” #include “kalman_filter.h” #include “motor.h” #include “encoder.h” #include “ultrasonic.h” #include “motion.h” #include “stdio.h” #include “string.h” / Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes */ // Ô²ÖùÐÅÏ¢½á¹¹Ìå #define MAX_CYLINDERS 10 typedef struct { int x, y, r; char color; } Cylinder; Cylinder cylinders[MAX_CYLINDERS]; int cylinder_count = 0; uint8_t mission_running = 0; uint32_t start_time = 0; #define TASK_TIMEOUT_MS 10000 #define WAIT_TASK_SELECT_TIMEOUT_MS 5000 uint32_t timeout = 10000; // 10s char openmv_buffer[128]; uint8_t openmv_index = 0; uint8_t rx_byte; uint8_t task_selected = 0; PID_Controller speed_pid; PID_Controller angle_pid; KalmanFilter kf; // ÈÎÎñģʽö¾Ù typedef enum { TASK_SIMPLE_EXIT, TASK_SNAKE_PATH, TASK_CIRCLE_TWO_COLORS, TASK_COLUMN_SWAP, TASK_RANDOM_DETECT, TASK_NONE } TaskMode; TaskMode current_task = TASK_NONE; // ״̬»ú¶¨Òå typedef enum { TASK_IDLE, TASK_MOVING_FORWARD, TASK_TURNING, TASK_AVOIDING, TASK_COMPLETED } TaskState; TaskState current_state = TASK_IDLE; /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------/ / USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------/ / USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------/ / USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); void StartMission(void); void ExecuteMission(TaskMode task); void SimpleExitPath(void); void SnakePath(void); void CircleTwoColors(void); void ColumnSwapPath(void); void RandomDetectPath(void); void AddCylinder(int x, int y, int r, char color); void BuildPath(char color); void AvoidObstacle(void); void Send_Debug_Info(void); void SensorFusion_Init(void); float SensorFusion_Update(float imu_yaw, float encoder_angle); void HAL_UART_RxCpltCallback(UART_HandleTypeDef huart); void UpdateCarState(void); uint8_t WaitForTaskSelection(uint32_t timeout_ms); / USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------/ / USER CODE BEGIN 0 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == &huart1) { if (rx_byte >= ‘1’ && rx_byte <= ‘5’) { current_task = (TaskMode)(rx_byte - ‘1’); // 1~5 Ó³É䵽ö¾Ù task_selected = 1; printf(“Selected Task: %d\r\n”, current_task + 1); StartMission(); } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //¼ÌÐø½ÓÊÕ } } // Æô¶¯ÈÎÎñ void StartMission(void) { mission_running = 1; current_state = TASK_MOVING_FORWARD; start_time = HAL_GetTick(); } // ÊÇ·ñ³¬Ê± uint8_t IsMissionTimeout() { return (HAL_GetTick() - start_time) > TASK_TIMEOUT_MS; } uint8_t WaitForTaskSelection(uint32_t timeout_ms) { uint32_t start = HAL_GetTick(); while (!task_selected && (HAL_GetTick() - start < timeout_ms)) { // ¿É¼ÓÈëÆäËû״̬¸üлò¿´ÃŹ· } return task_selected; } // Ìí¼ÓÔ²Öùµ½µØÍ¼ void AddCylinder(int x, int y, int r, char color) { if (cylinder_count < MAX_CYLINDERS) { cylinders[cylinder_count].x = x; cylinders[cylinder_count].y = y; cylinders[cylinder_count].r = r; cylinders[cylinder_count].color = color; cylinder_count++; } } // ¹¹½¨ÈÆÏß·¾¶ void BuildPath(char color) { if (color == ‘W’) { Turn_Left(90); } else if (color == ‘B’) { Turn_Right(90); } } //±ÜÕÏ void AvoidObstacle() { float left = Get_Left_Distance(); float right = Get_Right_Distance(); if (left < 10 && right < 10) { Motor_Stop(); printf("Obstacle detected in both directions. Stopping.\r\n"); } else if (left < 10) { Turn_Right(90); } else if (right < 10) { Turn_Left(90); } } //¼òµ¥Â·¾¶ void SimpleExitPath() { Move_Forward(100); Turn_Right(90); Move_Forward(50); } //ÉßÐη¾¶ void SnakePath() { for (int i = 0; i < cylinder_count; i++) { if (i % 2 == 0) { Turn_Left(90); } else { Turn_Right(90); } Move_Forward(20); } } //ÈÆÁ½¸öÑÕɫԲÖù void CircleTwoColors() { char colors_seen[2] = {0}; int count = 0; for (int i = 0; i < cylinder_count && count < 2; i++) { if (colors_seen[0] == 0 && cylinders[i].color == ‘W’) { BuildPath(‘W’); colors_seen[0] = ‘W’; count++; } else if (colors_seen[1] == 0 && cylinders[i].color == ‘B’) { BuildPath(‘B’); colors_seen[1] = ‘B’; count++; } } } //ºÚ°×Öù½»»»ºó·¾¶ void ColumnSwapPath() { //¼ÙÉèÒѽ»»»Öù×ÓÖØÐ¹滮·¾¶ BuildPath(‘W’); BuildPath(‘B’); } // Ëæ»ú̽²â·¾¶ static uint32_t detect_start_time = 0; static uint8_t detect_state = 0; void RandomDetectPath() {//½ÓÊÕOpenMVÊý¾Ý if (detect_state == 0) { HAL_UART_Receive_IT(&huart1, &rx_byte, 1); detect_start_time = HAL_GetTick(); detect_state = 1; } else if (detect_state == 1 && (HAL_GetTick() - detect_start_time > 30000)) { BuildPath(‘W’); Move_Forward(100); detect_state = 0; } } //Ö´ÐÐÈÎÎñ void ExecuteMission(TaskMode task) { switch (task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; default: printf(“Unknown task mode: %d\r\n”, task); break; } } //¸üÐÂС³µ×´Ì¬ void UpdateCarState() { if (!mission_running) return; switch (current_state) { case TASK_IDLE: //¿ÕÏÐ״̬ break; case TASK_MOVING_FORWARD: //Ö´ÐÐÈÎÎñ switch (current_task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; } current_state = TASK_COMPLETED; break; case TASK_COMPLETED: Motor_Stop(); mission_running = 0; current_state = TASK_IDLE; break; } } /* USER CODE END 0 */ /** @brief The application entry point. @retval int / int main(void) { / USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals / MX_GPIO_Init(); MX_I2C1_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM4_Init(); MX_USART1_UART_Init(); / USER CODE BEGIN 2 */ //³õʼ»¯OpenMV½ÓÊÕ HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //Æô¶¯±àÂëÆ÷¼ÆÊý HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); // ³õʼ»¯ IMU JY61P_Init(&hi2c1); // ³õʼ»¯ PID PID_Init(&speed_pid, 1.0f, 0.1f, 0.05f); PID_Init(&angle_pid, 2.0f, 0.0f, 0.1f); // ³õʼ»¯ ¿¨¶ûÂüÂ˲¨ KalmanFilter_Init(&kf, 0.001f, 0.003f, 0.03f); // Æô¶¯ PWM Êä³ö HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); //Ìí¼Ó²âÊÔÔ²Öù AddCylinder(100, 200, 30, ‘W’); AddCylinder(300, 150, 35, ‘B’); // µÈ´ýÈÎÎñ if (!WaitForTaskSelection(WAIT_TASK_SELECT_TIMEOUT_MS)) { current_task = TASK_SIMPLE_EXIT; } /* USER CODE END 2 */ /* Infinite loop / / USER CODE BEGIN WHILE */ while (1) { } //¸üÐÂ״̬»ú UpdateCarState(); //³¬Ê±¼ì²â if (IsMissionTimeout()) { mission_running = 0; Motor_Stop(); printf("Mission Timeout!\r\n"); } /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ /* USER CODE END 3 */ } /** @brief System Clock Configuration @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Initializes the RCC Oscillators according to the specified parameters in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ /* USER CODE END 4 */ /** @brief This function is executed in case of error occurrence. @retval None / void Error_Handler(void) { / USER CODE BEGIN Error_Handler_Debug / / User can add his own implementation to report the HAL error return state / __disable_irq(); while (1) { } / USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** @brief Reports the name of the source file and the source line number where the assert_param error has occurred. @param file: pointer to the source file name @param line: assert_param error line source number @retval None / void assert_failed(uint8_t file, uint32_t line) { / USER CODE BEGIN 6 / / User can add his own implementation to report the file name and line number, ex: printf(“Wrong parameters value: file %s on line %d\r\n”, file, line) / / USER CODE END 6 / } #endif / USE_FULL_ASSERT / gpio.c / USER CODE BEGIN Header / / @file gpio.c @brief GPIO ³õʼ»¯ @attention Ó²¼þ:TB6612FNG£¬HC-SR04£¬I2C£¬USART1 / / USER CODE END Header */ #include “gpio.h” /----------------------------------------------------------------------------/ /* Configure GPIO / /----------------------------------------------------------------------------*/ void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /-------------------------- TB6612FNG ¿ØÖÆÒý½Å ------------------------------/ /** PWMA - PB1 (TIM2_CH2) AIN1 - PB14 AIN2 - PB15 PWMB - PB0 (TIM2_CH1) BIN1 - PB12 BIN2 - PB13 STBY - 3.3V */ GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_12 | GPIO_PIN_13; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- PWM Êä³ö PB0, PB1 ---------------------------/ /** ??? PWM ??(TIM2_CH1 ? CH2) */ GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- HC-SR04 ×ó TRIG & ECHO ---------------------------/ /** TRIG - PB2 ECHO - PB3 */ GPIO_InitStruct.Pin = GPIO_PIN_2; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_3; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- HC-SR04 ÓÒ TRIG & ECHO ---------------------------/ /** TRIG - PB4 ECHO - PB5 */ GPIO_InitStruct.Pin = GPIO_PIN_4; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- I2C1 SCL & SDA ----------------------------------/ /** SCL - PB8 SDA - PB9 */ GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- USART1 TX & RX ----------------------------------/ /** TX - PA9 RX - PA10 */ GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); } motion.c #include “motion.h” #include “encoder.h” #include “motor.h” #include “jy61p.h” #include “math.h” #include “stddef.h” #include “pid.h” void Move_Forward(int distance_cm) { int target_ticks = distance_cm * 10; Reset_Encoders(); Motor_Forward(100); while (1) { int left = Get_Left_Encoder(); int right = Get_Right_Encoder(); int avg = (left + right) / 2; if (avg >= target_ticks) { Motor_Stop(); break; } } } void Turn_Left(int angle_deg) { float target_yaw = angle_deg; float current_yaw = 0.0f; JY61P_Data imu_data; PID_Controller yaw_pid; PID_Init(&yaw_pid, 2.0f, 0.0f, 0.1f); while (fabsf(current_yaw) < target_yaw) { JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; float error = target_yaw - current_yaw; float output = PID_Update(&yaw_pid, error, 0.01f); Motor_Turn_Left((int)(output)); } Motor_Stop(); } void Turn_Right(int angle_deg) { float target_yaw = angle_deg; float current_yaw = 0.0f; JY61P_Data imu_data; PID_Controller yaw_pid; PID_Init(&yaw_pid, 2.0f, 0.0f, 0.1f); while (fabsf(current_yaw) < target_yaw) { JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; float error = target_yaw - current_yaw; float output = PID_Update(&yaw_pid, error, 0.01f); Motor_Turn_Right((int)(output)); } Motor_Stop(); } pid.c #include “pid.h” void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd) { pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->integral = 0.0f; pid->last_error = 0.0f; } float PID_Update(PID_Controller *pid, float error, float dt) { pid->integral += error * dt; float derivative = (error - pid->last_error) / dt; float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative; pid->last_error = error; return output; } ultrasonic.c #include “ultrasonic.h” #include “tim.h” #include “gpio.h” float Get_Left_Distance() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_SET); HAL_Delay(10); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_RESET); uint32_t start = 0, end = 0; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_RESET); start = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } float Get_Right_Distance() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET); HAL_Delay(10); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); uint32_t start = 0, end = 0; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_RESET); start = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } kalman_filter.c #include “kalman_filter.h” void KalmanFilter_Init(KalmanFilter *kf, float Q_angle, float Q_bias, float R_measure) { kf->Q_angle = Q_angle; kf->Q_bias = Q_bias; kf->R_measure = R_measure; kf->angle = 0.0f; kf->bias = 0.0f; kf->P[0][0] = 0.0f; kf->P[0][1] = 0.0f; kf->P[1][0] = 0.0f; kf->P[1][1] = 0.0f; } float KalmanFilter_Update(KalmanFilter *kf, float new_angle, float new_rate, float dt) { kf->rate = new_rate - kf->bias; kf->angle += dt * kf->rate; kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= dt * kf->P[1][1]; kf->P[1][0] -= dt * kf->P[1][1]; kf->P[1][1] += kf->Q_bias * dt; float y = new_angle - kf->angle; float S = kf->P[0][0] + kf->R_measure; float K[2]; K[0] = kf->P[0][0] / S; K[1] = kf->P[1][0] / S; kf->angle += K[0] * y; kf->bias += K[1] * y; float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K[0] * P00_temp; kf->P[0][1] -= K[0] * P01_temp; kf->P[1][0] -= K[1] * P00_temp; kf->P[1][1] -= K[1] * P01_temp; return kf->angle; } motor.c #include “motor.h” #include “tim.h” void Motor_SetSpeed(int left, int right) { __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, left); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, right); } void Motor_Forward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); // AIN1 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); // AIN2 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); // BIN1 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); // BIN2 Motor_SetSpeed(speed, speed); } void Motor_Backward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); Motor_SetSpeed(speed, speed); } void Motor_Stop() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); Motor_SetSpeed(0, 0); } void Motor_Turn_Left(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); Motor_SetSpeed(speed, speed); } void Motor_Turn_Right(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); Motor_SetSpeed(speed, speed); } encoder.c #include “encoder.h” #include “tim.h” void Reset_Encoders(void) { htim3.Instance->CNT = 0; htim4.Instance->CNT = 0; } float Get_Total_Distance(void) { int32_t left_ticks = htim3.Instance->CNT; int32_t right_ticks = htim4.Instance->CNT; float distance = (left_ticks + right_ticks) / 2.0f * 0.1f; return distance; } float Get_Left_Encoder(void) { return (float)htim3.Instance->CNT; } float Get_Right_Encoder(void) { return (float)htim4.Instance->CNT; } jy61p.c #include “jy61p.h” #include <string.h> #include “i2c.h” #define JY61P_ADDR 0x50 << 1 void JY61P_Init(I2C_HandleTypeDef *hi2c) { } void JY61P_Read_Acc(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x34, 1, buf, 6, HAL_MAX_DELAY); data->ax = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 16; data->ay = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 16; data->az = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 16; } void JY61P_Read_Gyro(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x38, 1, buf, 6, HAL_MAX_DELAY); data->gx = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 2000; data->gy = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 2000; data->gz = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 2000; } void JY61P_Read_Attitude(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x56, 1, buf, 6, HAL_MAX_DELAY); data->pitch = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 180; data->roll = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 180; data->yaw = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 180; } usart.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file usart.c * @brief This file provides code for the configuration * of the USART instances. ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "usart.h" #include "stdio.h" #include "string.h" /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ UART_HandleTypeDef huart1; /* USART1 init function */ void MX_USART1_UART_Init(void) { /* USER CODE BEGIN USART1_Init 0 */ /* USER CODE END USART1_Init 0 */ /* USER CODE BEGIN USART1_Init 1 */ /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; huart1.Init.BaudRate = 115200; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; huart1.Init.Mode = UART_MODE_TX_RX; huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart1.Init.OverSampling = UART_OVERSAMPLING_16; if (HAL_UART_Init(&huart1) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN USART1_Init 2 */ /* USER CODE END USART1_Init 2 */ } void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; if(uartHandle->Instance==USART1) { /* USER CODE BEGIN USART1_MspInit 0 */ /* USER CODE END USART1_MspInit 0 */ /* USART1 clock enable */ __HAL_RCC_USART1_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX */ GPIO_InitStruct.Pin = GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* USART1 interrupt Init */ HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspInit 1 */ /* USER CODE END USART1_MspInit 1 */ } } void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) { if(uartHandle->Instance==USART1) { /* USER CODE BEGIN USART1_MspDeInit 0 */ /* USER CODE END USART1_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_USART1_CLK_DISABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); /* USART1 interrupt Deinit */ HAL_NVIC_DisableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspDeInit 1 */ /* USER CODE END USART1_MspDeInit 1 */ } } /* USER CODE BEGIN 1 */ void Send_Debug_Info(void) { char buffer[100]; sprintf(buffer, "Left: %.2f cm, Right: %.2f cm\r\n", Get_Left_Distance(), Get_Right_Distance()); HAL_UART_Transmit(&huart1, (uint8_t*)buffer, strlen(buffer), HAL_MAX_DELAY); } /* USER CODE END 1 */ 这个代码可以实现的功能,接线
08-01
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值