2022电赛省c题一小车跟随行驶系统基于stm32f407

最新推荐文章于 2025-05-09 09:33:49 发布

电赛张小七

最新推荐文章于 2025-05-09 09:33:49 发布

阅读量2.3k

点赞数 30

文章标签： c语言开发语言单片机 stm32 python 嵌入式硬件

本文链接：https://blog.youkuaiyun.com/weixin_60991529/article/details/139232795

版权

做这个项目之前先要明白本次的任务和要求是什么，下面是本次项目的任务和要求

一、任务

设计一套小车跟随行驶系统，采用 TI 的 MCU ，由一辆领头小车和一辆跟随

小车组成，要求小车具有循迹功能，且速度在 0.3 ~ 1m/s 可调，能在指定路径上

完成行驶操作，行驶场地的路径如图 1 所示。其中，路径上的 A 点为领头小车

每次行驶的起始点和终点。当小车完成一次行驶到达终点，领头小车和跟随小车

要发出声音提示。领头小车和跟随小车既可以沿着 ABFDE 圆角矩形（简称为

内圈）路径行驶，也可以沿着 ABCDE 的圆角矩形（简称为外圈）路径行驶。

当行驶在内圈 BFD 段时，小车要发出灯光指示。此外，在测试过程中，可以在

路径上 E 点所在边的直线区域，由测试专家指定位置放上 “ 等停指示 ” 标识（见图

1 左侧），指示领头小车在此处须停车，等待 5 秒后再继续行驶。

二、要求

1. 将领头小车放在路径的起始位置 A 点，跟随小车放在其后 20cm 处，设

定领头小车速度为 0.3m/s ，沿着外圈路径行驶一圈停止，要求：（ 20 分）

(1) 领头小车的平均速度误差不大于 10% ；

(2) 跟随小车能跟随领头小车行驶，全程不能发生小车碰撞；

(3) 完成一圈行驶后领头小车到达 A 点处停车，跟随小车应及时停止，停止

时间差不超过 1s ，且与领头小车的间距为 20cm ，误差不大于 6cm 。

2. 将领小车放在路径轨迹的起始位置 A 点，跟随小车放在路径上 E 点所在

边的直线区域，由测试专家指定的位置，设定领头小车速度为 0.5m/s ，沿着外圈

路径行驶两圈停止，要求：（ 20 分）

(1) 领头小车的平均速度误差不大于 10% ；

(2) 跟随小车能快速追上领头小车，然后按 20cm 间距跟随领头小车行驶，

全程不能发生小车碰撞：

(3) 完成两圈行驶后领头小车达到 A 点停止，跟随小车应及时停止，两车停

止的时间差不超过 1s ，且与领头小车的间距为 20cm ，误差不大于 6cm 。

3. 将领头小车放在路径的起始位置 A 点，跟随小车放在其后 20cm 处，领

头小车和跟随小车连续完成三圈路径的行驶。第一圈领头小车和跟随小车都沿着

外圈路径行驶。第二圈领头小车沿着外圈路径行驶，跟随小车沿着内圈路径行驶，

实现超车领跑。第三圈跟随小车沿着外圈路径行驶，领头小车沿着内圈路径行驶，

实现反超和再次领跑。要求：（ 30 分）

(1) 全程两个小车行驶平稳，顺利完成两次超车，且不能发生小车碰撞；

(2) 完成三圈行驶后领头小车到达 A 点停止，跟随小车应及时停止，两车停

止的时间差不超过 1s ，且与领头小车的间距为 20cm ，误差不大于 6cm ；

(3) 小车行驶速度可自主设定，但不得低于 0.3m/s ，且完成所规定的三圈轨

迹行驶所需时间越短越好。

4. 由测试专家在路径的 E 点所在边的直线区域指定位置，放上 “ 等停指示 ”

标识。然后，将领头小车放在路径的起始位置 A 点，跟随小车放在其后 20cm 处，

设定领头小车速度为 1m/s ，沿着外圈路径行驶一圈，行驶中两小车不得发生碰

闯。要求：（ 20 分）

三、难点

看完了项目任务和对应的要求之后，感觉没有什么难点，但是细看要求三和要求四其实有一定的难度的，在整个前期的过程中要求一二都是比较好完成的，要求三的难点在于两车之间的配合，（此次项目只有后车装了超声波）在实际调整的过程中这里也是出现了一点小问题，要么后车超前车的时候前车速度过快后车追不上，导致在汇合的路口两车相撞。要么就是第三圈的时候前车速度不够，导致在相汇合的路口相撞。造成这种因素还是两车的配合不够好，两车的pid调的不够匀速。但也不可能非常匀速的，因为受到电机各种的硬件因素，只能相对匀速。

然后整个项目的过程也是一直在速度和配合之间相互的磨合，因为看着题目上面说速度越快越好，一开始就是一直追求高速度，达到超过发挥项的要求。最开始是0.7米每秒的速度开始跑要求三，但是结果一直都不理想，要么后车撞到前车，要么前车撞到后车，等等一系列的问题，于是不得不妥协，开始下降速度最终是在0.5米每秒，效果预期还不错，然后就开始调整两车之间的配合。发现前车和后车在走内环超车的时候偶尔还是会相撞。针对这个于是就开始了一系列的尝试，最后让不管前车还是后车在走内环超车的时候加一些速度，尽量保持匀速，这样就可以较好的避免这个问题。当然最后的解决这个问题是在前车上也安装一个超声波，这样就从根本上面解决了这个问题。

最后一个难点就是发挥项，速度达到了要求该怎么才能停车，也就是怎么做才能停准、停好，不超过线。这个问题当时也是尝试了很多方法，比喻说直接给关闭电机（这样会滑行很长的一段距离，超过停车线）再比如说把判断停车的框画的靠上一些，这样偶尔会不准（因为越靠上，openmv的识别准度越低）再比如说给电机翻转来抵消这个车向前移动的惯性，这样会让车停歪也不行。最后就直接给pwm 0值关闭转向换，然后给速度环一个较大的p值这样就可以快速稳稳的停在要求范围内。

四、调节过程

整个pid的调整过程还是比较轻松的，因为调车的pid比较容易。整个大题的思路还是老样子，先调好速度环，再调角度环，让两者共同作用，达到预期效果。我两个pid是并行的直接相加的，串行的相对而言效果更好，但是并行的最终也能达到最后的目的。

首先就是速度环的调节，当然你要想调好它，就需要知道达到什么样子才是调好了。当你设定一个目标值，它很快就能达到那个效果，而不是一下一下的慢慢提速上去的，就像零百加速2.78s那个样子（当然这只是一个比喻）。

直观一点效果就是你设定目标速度50，然后编码器上面能够在1秒内到达一个顶峰，速度不在增加，而是在一个范围内变化（因为你设定速度50，实际速度不可能直接达到50，一般电机应该是低于50，但也不会相差的太多，一般而言在45左右）达到这个效果说明你第一个环已经没有多大的问题，如果达不到这一效果，也可以退而求其次就是达到你个小车一个目标速度，小车能够走直线即可。达到这个程度速度低看不出来问题，认为第一个环已经调好了，前面三个要求可以轻松过关，但是到了第四个要求你就会很清晰的发现你的小车有很明显的一段加速时间，这样就会导致小车不稳，或者开跑就死的一些状况。

第一个环调好了，就尽量不要动了，就开始调节第二个环。（i值在这两个环中的意义不大，有时还可能有反作用，所有我两个环都没有用i，直接用的pd）首先就是如何判断角度环的p值的大小。（小车能够正常巡线的情况下）如下图所示，如果p偏小小车就会在过弯道的时候整个车往右偏，或者右边轮子直接压着黑线，如果p大了就会往左偏这个是最直观的现象。

有时车子会转圈圈，这个时候转圈圈的情况就有很多种，p过大会导致转圈圈，d过大寻不到线的时候也会转圈圈。整个调节的过程速度从小到大的去调整，这样更好调，pd也是慢慢增加的调整。整个过程中尽量不去更改第一个环，最后就是调节的过程中只保证一个变量，也就是一次只调节一个相对应的p或者d，这样你就可以非常直观的看出来p或者d，调整后的整体结果，也就可以知道其的大小。

五、程序源码

最后程序源码给到各位，整个项目下来需要解决的问题不多，当然程序源码只是一个参考。

编码器度数：

#include "encoder.h"
#include "stm32f4xx.h" 
#include "pwm.h"
#include "pid.h"
#include"usart.h" 
int readshuduzuo,readshuduyou;
extern int y,x,biaozhi,shu,r,v,mubiaoshudu,a1,zuikuai1,zuikuai2,c1,w,da;
extern float Velocity_Kp,Velocity_Ki,Vertical_Kp,Vertical_Kd,Velocity_Kd;
PA0,1串口无法拉高
编码器左 tim4 ab相：b67
int tiaochu=0,c=0;
void Encoder_TIM4_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB ,ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;  //TIM4_CH1  TIM4_CH2
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4); 
	GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4); 	
	                                                                                                                                                                                                                                                                              
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 0xffff;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 0x00;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);
	
	//编码器模式
	TIM_ICInitTypeDef TIM_ICInitStructure;
    TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//TIM_ICPolarity_Rising捕获发生在ICx的上升沿
    TIM_ICStructInit(&TIM_ICInitStructure); //将结构体中的内容缺省输入
    TIM_ICInitStructure.TIM_ICFilter = 0;//滤波器值
    TIM_ICInit(TIM4, &TIM_ICInitStructure);  
	
	TIM_ITConfig(TIM1,TIM_IT_Update,ENABLE);
	
	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; 
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;  
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=1; 
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	
	TIM_SetCounter(TIM4, 8400);
	TIM_Cmd(TIM4, ENABLE);
}
void TIM4_IRQHandler(void)
{ 		    
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //溢出中断
	{
		
	}
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //清除中断标志位 					  
}







//编码器右  tim3  ab相：a67




void Encoder_TIM3_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA ,ENABLE);
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;  //TIM4_CH1  TIM4_CH2
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource6,GPIO_AF_TIM3); 
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM3); 	
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 0xffff;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 0x00;
	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);
	
	//编码器模式
	TIM_ICInitTypeDef TIM_ICInitStructure;
    TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//TIM_ICPolarity_Rising捕获发生在ICx的上升沿
    TIM_ICStructInit(&TIM_ICInitStructure); //将结构体中的内容缺省输入
    TIM_ICInitStructure.TIM_ICFilter = 0;//滤波器值
    TIM_ICInit(TIM3, &TIM_ICInitStructure);  
	
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);
	
	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; 
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;  
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=1; 
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	
	TIM_SetCounter(TIM3, 0);
	TIM_Cmd(TIM3, ENABLE);
}
void TIM3_IRQHandler(void)
{ 		    
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中断
	{
		
	}
	TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //清除中断标志位 					  
}


void TIM2_Int_Init(u16 arr,u16 psc)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM3时钟
	
  TIM_TimeBaseInitStructure.TIM_Period = arr; 	//自动重装载值
	TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
	
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
	
	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器3更新中断
	TIM_Cmd(TIM2,ENABLE); //使能定时器3
	
	NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器3中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x03; //抢占优先级1
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	
}
void TIM2_IRQHandler(void)//定时(10ms)读取编码器值(即速度),超声波测距
{
	if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET) //溢出中断
	{	shu=shu+1;
		readshuduzuo=(int16_t)TIM_GetCounter(TIM4);
		readshuduyou=(int16_t)TIM_GetCounter(TIM3);
			TIM_SetCounter(TIM4,0);
			TIM_SetCounter(TIM3,0);
		if(biaozhi==1&&tiaochu==0){
			v=Velocity(mubiaoshudu,readshuduzuo,readshuduyou);
			r=Vertical(80,y);
			Set_Pwm(r+v,-r+v);
									}
		
									
		//1停止
		if (a1==1 && x==2 && biaozhi==1)
		{	tiaochu=1;
			//Set_Pwm(0,0);
		
			Velocity_Ki=0;
			v=Velocity(0,readshuduzuo,readshuduyou);
			Set_Pwm(v,v);
			if(readshuduzuo==0 ||readshuduyou==0)
				{
						Set_Pwm(0,0);
				}		
		}
		
		
		//2停止
		if (a1==2 && x==4 && biaozhi==1&&da==0)
		{	tiaochu=1;
			Set_Pwm(0,0);
		}
		//3停
		
		if (a1==3 && w==5 && biaozhi==1&&da==1)
		{Serial_SendByte(1);
			c=1;
			tiaochu=1;
			Vertical_Kp=0;
			Vertical_Kd=0;
			Set_Pwm(0,0);
		}
		
		if (a1==0&&x==1&&biaozhi==1&&da==0)
		{Serial_SendByte(1);
			c1=1;
			tiaochu=1;
			Vertical_Kp=0;
			Vertical_Kd=0;
			Velocity_Kp=-20000;
		
			Velocity_Ki=0;
			v=Velocity(0,readshuduzuo,readshuduyou);
			Set_Pwm(v,v);
		}
		
		
		
	}
		
	
	
	TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位
}

pid的调整：

#include "pid.h"
#include "pwm.h"
#include "encoder.h"
extern int readshuduzuo,readshuduyou,x,y,w,h,Cx,Cy,Ch,Cw;

float 
  Vertical_Kp=0,			//shudu20  12(0.3m/s)               shudu30  20(0.5m/s)  shudu40  20       shudu45     30                     shudu50   30       shudu55     45                        75 
	Vertical_Ki=0,
  Vertical_Kd=0			//         5                                 100                    220                    350                          530                    770                     1600
;  
float 
  Velocity_Kp=-3000,            //3000
  Velocity_Ki=1,               //1
	Velocity_Kd=150                //150
; 

//角度环
int Vertical(float Med,float Angle) 
{static float error,error_last;
  int PWM_out;
	error=Angle-Med;

  PWM_out = Vertical_Kp*error+Vertical_Kd*(error-error_last);
	error_last=error;
  return PWM_out;
} 

//位置环
int Velocity(int Target,int speedleft,int speedright)
{

//定义静态变量，保存在静态存储器，使得变量不丢掉
	static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last,err_last;
	float a=0.7;
	
//1.计算速度偏差
//舍去误差--能够让速度为0的角度，就是机械中值
	Encoder_Err = (speedright+speedleft)/2-Target;
		
//2.对速度误差进行低通滤波
//Low_out=(1-a)*Ek+a*Low_out_Last
	EnC_Err_Lowout = (1-a)*Encoder_Err+a*EnC_Err_Lowout_last;       //使波形更加平滑，滤除高频干扰，防止速度突变
	
//3.对速度偏差积分出位移
	Encoder_S += EnC_Err_Lowout;
	
//4.积分限幅
	Encoder_S=Encoder_S>1000?1000 : (Encoder_S<(-1000)?(-1000) : Encoder_S);   //10000
	
//5.速度环控制输出
	PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S+Velocity_Kd*(Encoder_Err-err_last);
err_last=Encoder_Err;
	return PWM_out;
  
}

主函数部分：

#include "stm32f4xx.h"
#include "usart.h"
#include "delay.h"
#include "oled.h"
#include "pwm.h"
#include "led.h"
#include "encoder.h"
#include "pid.h"
#include "openmv.h"
extern int readshuduzuo,readshuduyou,shu;
extern int x,y,w,h,biaozhi,c;
extern float Vertical_Kp,Vertical_Kd,Velocity_Ki,Velocity_Kd,Velocity_Kp;
int shu=0;
int v=0,r=0,c1=0;
int mubiaoshudu=0;
int a1=0,zuikuai1=0,zuikuai2=0;
int b1=0,b2=0,b3=0,b4=0,da=0;
int main(void)
{	 u8 key;    
	uart_init(115200);
	usart6_init(115200);
  USART_Cmd(USART6,ENABLE);

	delay_init(168);
	
	TIM14_PWM_Init(8400,42);
	TIM13_PWM_Init(8400,42);
	
	SERVE_Init();
	SERVE_Init1();
	
	Encoder_TIM4_Init();
	Encoder_TIM3_Init();
	
	TIM2_Int_Init(8400-1,50-1);
	OLED_Init();
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	TIM_Cmd(TIM13, ENABLE);
	TIM_Cmd(TIM14, ENABLE);
	
	KEY_Init();
	
	int bex=0;
	
  while(1){
		OLED_ShowSNum(16,0,readshuduyou,4,16,1);
		OLED_ShowSNum(32,0,readshuduzuo,4,16,1);  
		OLED_ShowSNum(0,16,y,4,16,1);	
		OLED_ShowSNum(0,48,key,4,16,1);  
		OLED_ShowSNum(16,48,w,4,16,1);  
		OLED_ShowSNum(0,32,x,4,16,1);	
		OLED_ShowSNum(16,32,a1,4,16,1);	
		OLED_Refresh();	
		
		
		key=KEY_Scan(0);		//得到键值
	   	if(key)
		{						   
			switch(key)
			{				 
				case WKUP_PRES:
					mubiaoshudu=30;
					Vertical_Kp=20;
					Vertical_Kd=100;
					a1=3;
					da=1;
					break;
				case KEY0_PRES:	//控制LED0翻转
					mubiaoshudu=20;
					Velocity_Kd=0;
					Velocity_Kp=-2000;
					Vertical_Kp=15;
					Vertical_Kd=0;
					
					a1=1;
					
					break;
				
				case KEY1_PRES:	//控制LED1翻转	 
					mubiaoshudu=30;
					Vertical_Kp=20;
					Vertical_Kd=100;
					a1=2;
					break;
			}
		}
		if(a1==1&&biaozhi==1&&b1==0)
		{	Serial_SendByte(4);
			b1=1;}
		
		if(a1==2&&biaozhi==1&&b2==0)
		{	Serial_SendByte(5);
			b2=1;}
		
		if(a1==3&&biaozhi==1&&c==0&&b3==0)
		{	delay_ms(500);
			Serial_SendByte(6);
			b3=1;}
	
		if (a1==3&&h==1&&bex==0)
		{			delay_ms(300);
					mubiaoshudu=51;
					Vertical_Kp=40;
					Vertical_Kd=530;
					bex=1;
		}
		if(a1==3&&h==2)
		{
					delay_ms(350);
					mubiaoshudu=30;
					Vertical_Kp=20;
					Vertical_Kd=100;
		
		}

		if(a1==0&&biaozhi==1&&c1==0&&b4==0)
		{			mubiaoshudu=51;
					Vertical_Kp=40;
					Vertical_Kd=530;
					delay_ms(50);
					Serial_SendByte(7);
					b4==1;
		}
	
	}
}

openmv部分：

import sensor, image, time,math,pyb
from pyb import UART,LED
import json
import ustruct
import pyb
#THRESHOLD = (91, 3)    #二分化系数
ROI=(2,84,157,26)
BINARY_VISIBLE = True
sensor.reset()
sensor.set_vflip(True)          #垂直翻转
sensor.set_hmirror(True)        #水平翻转
sensor.set_pixformat(sensor.RGB565)         #彩色
#sensor.set_pixformat(sensor.GRAYSCALE)     #黑白
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
RED_threshold=(6, 19, -39, 22, -38, 90)
clock = time.clock()
ROIXIN=(2,84,157,26)
ROITING=(1,1,156,11)#停车
ROI2=(2,51,158,18)
ROI3=(2,29,156,15)
uart = UART(3,115200)
uart.init(115200,bits=8,parity=None,stop=1)
ting=0
twice1=0
twice2=0
huan=1
san1=0
san2=0
san3=0
san4=0
quanshu=0
x=0
bx=0
ting1=0
xx=0
xx1=0
jia=0
er1=0
er2=0
jia1=0
while(True):
    clock.tick()
    img = sensor.snapshot()
    #寻找色块
    img.draw_rectangle(ROIXIN)
    img.draw_rectangle(ROITING)
    img.draw_rectangle(ROI2)
    img.draw_rectangle(ROI3)
    blobn1=img.find_blobs([RED_threshold],roi=ROITING)
    if blobn1:
       largest_blobn1 = max(blobn1,key=lambda c:c.pixels())
       img.draw_rectangle(largest_blobn1.rect())
       print(largest_blobn1.pixels(),largest_blobn1.h(),largest_blobn1.w(),ting)
       if largest_blobn1.pixels()>200 and largest_blobn1.h()>7  and largest_blobn1.w()>38:
            ting=1
    blobn2=img.find_blobs([RED_threshold],roi=ROI2)
    if blobn2:
       largest_blobn2 = max(blobn2,key=lambda c:c.pixels())
       img.draw_rectangle(largest_blobn2.rect())
      # print(largest_blobn2.pixels(),largest_blobn2.h(),largest_blobn2.w(),ting)
       if largest_blobn2.pixels()>700:
            twice1=1
       if twice1==1:
            if largest_blobn2.pixels()<700:
                twice2=1
       if twice1==1 and twice2==1 and largest_blobn2.pixels()>700:
            ting=4
       if ting==4 and largest_blobn2.pixels()<700:
            er1=1
       if er1==1 and largest_blobn2.pixels()>700:
            ting1=5
    blobn3=img.find_blobs([RED_threshold],roi=ROI3)
    if blobn3:
       largest_blobn3 = max(blobn3,key=lambda c:c.pixels())
       img.draw_rectangle(largest_blobn3.rect())
       print(largest_blobn3.pixels(),largest_blobn3.h(),largest_blobn3.w(),ting)
       if largest_blobn3.pixels()>350:
         san1=1
         ting=1
       if san1==1:
            if largest_blobn3.pixels()<350:
                san2=1
       if san1==1 and san2==1 and  largest_blobn3.pixels()>350:
            quanshu=2
            san3=1
       if san1==1 and san2==1 and  largest_blobn3.pixels()<350 and san3==1:
            san4=1
       if largest_blobn3.pixels()>350 and quanshu==2 and san4==1 and largest_blobn3.w()>50:
            xx=1
       if xx==1 and largest_blobn3.pixels()<350 :
            xx1=1
       if jia==1 and largest_blobn3.pixels()>350  and jia1==1:
                jia=2
    blobn=img.find_blobs([RED_threshold],roi=ROIXIN,x_stride=10,y_stride=10)
    for a in blobn:

        if a.pixels()>1500:
            ting=2
        if len(blobn)==2:
            print(blobn[0].cx(),blobn[1].cx())
        if len(blobn)==2:
            if quanshu==2 and jia1==0:
                 jia=1
                 jia1=1
            if quanshu==2:
                 output_str=bytearray([0x2c,0x12,ting,min(blobn[0].cx(),blobn[1].cx()),ting1,jia,0x5B])
                 uart.write(output_str)
            else:
                output_str=bytearray([0x2c,0x12,ting,max(blobn[0].cx(),blobn[1].cx()),ting1,jia,0x5B])
                uart.write(output_str)
        else:
            output_str=bytearray([0x2c,0x12,ting,a.cx(),ting1,jia,0x5B])
            uart.write(output_str)
            print(a.pixels())

openmv部分有点杂乱，没来得及及时整理，只是简单的移过来了，其中有许多部分可以删减，或者更改。

以上为本次项目的部分代码，详细的工程文件在后面一条文章中。