智能车竞赛技术报告 | 智能车视觉 - 石家庄学院 - 百事

Posted 2021-09-21 卓晴

简 介： 本文以第十六届全国大学生智能汽车竞赛为背景介绍了智能赛车控制系统的软硬件结构和开发流程。
本文设计的智能车系统以MIMXRT1064 芯片作为系统的控制核心，进行信号采样、 数据处理运算，通过控制其产生的 PWM 波完成对舵机和电机的控制。利用摄像头采集赛道图像，并在车模轮轴处安装编码器用于采集电机转速，完成车模的姿态及速度的采集，进而控制小车的舵机与电机，使小车能在赛道上稳定高速的完成比赛任务。

关键词： 智能汽车，摄像头，神经网络，深度学习，PID，MIMXRT1064

学校：石家庄学院    
队伍名称：百事       
参赛队员：刘浩 郝学辉 张雪然
带队教师：杨丽 姜辉     

 

引言

---

全国大学生智能汽车竞赛是以“立足培养、重在参与、鼓励探索、追求卓越”为宗旨，鼓励创新的一项科技竞赛活动。竞赛中智能视觉组要求在规定的C型车模平台上，以恩智浦半导体公司的微控制器作为核心控制模块，通过自主构思车模控制方案进行系统设计，包括传感器信号采集处理、电机驱动、转向舵机控制以及控制算法软件开发等，制作完成一个能够自主识别道路、路边目标识别与定位、选择三岔路口方向的模型汽车。

针对赛题要求，我们建立了车模、道路、路边目标和自动控制系统的基本模型，给出了理论分析、仿真计算、在线调试的基本开发方法。在比较各种算法的性能特点后，我们确定了最终方案，并完成了车模的制作与调试。智能车开发过程中，通过比较各种方案，最终我们决定采用MT9V034摄像头作为赛道元素采集传感器，通过 PID算法进行引导，同时采用基于OPENART mini的深度学习完成路边目标识别与定位以及三岔路口选择，从而完成智能视觉组的赛道任务。

 

第一章 方案设计

---

整体方案设计

▲ 图 1 系统概述

车模系统整体结构如图1所示。各模块作用:

(1) MIMXRT1064模块:作为智能车的控制中心摄像头陀螺仪按键采集到的信号根据控制算法做出相应的控制策略,输出PWM波,使得小车做出相应调整。
(2) 摄像头模块:作为车模的眼睛，实时的采集赛道图像，为车模的方向环稳定作保障。
(3) OPENART mini模块: 辅助摄像头采集赛道信号，完成路边目标识别与定位以及三岔路口选择。
(4) 陀螺仪模块:辅助摄像头采集赛道信号，采集摄像头不易察觉的赛道信号。
(5) 编码器模块:采集电机转速，为车模的速度环稳定作保障。
(6) 上位机模块：包括拨码开关和按键，方便调整各个参数。
(7) 电源模块:为整个系统提供稳定合适的电源供给。

 

第二章 机械结构

---

根据智能视觉组比赛规则，我们使用C型车模参加竞赛。我们对车模进行了详细的系统分析，最终我们认为要在规则允许范围内改造车模，以提高车模整体精度，提高车辆行驶的稳定性。前轮定位的作用是保障汽车直线行驶的稳定性，转向轻便和减少轮胎的磨损。前轮是转向轮，它的安装位置由主销内倾、主销后倾、前轮外倾和前轮前束等4种可调参数决定，实现转向轮、主销和前轴等三者在车架上的位置关系。

2.1机械建模

我们组采用了博思电子数码科技有限公司生产的C型车模。

2.2 主销后倾

主销后倾是指主销轴线上端在纵向平面内略向后倾斜的现象。主销后倾角越大，车速越高， 稳定力矩越大，前轮稳定性也越好。但是稳定力矩也不宜过大，否则在转向时为了克服稳定力矩，舵机需要施加更大的力。 主销后倾可通过调整主销下端垫片数量和厚度实现。

2.3前轮内倾

从车前方看轮胎时，主销轴向车身内测倾斜，该角度称为主销内倾角。

前轮外倾的作用:可以抵消由于车的重力使车轮向内倾斜的趋势，减小车模机件的磨损与负重，使转向灵活。

当车轮以主销为中心回转时，车轮的最低点将陷入路面以下，实际上车轮不可能陷入路面以下，而是将转向轮连同整车身前部向上抬起一个相应的高度，这样车模本身的重力有使转向车轮回复到原来中间位置的效应，因而舵机容易复位。但主销内倾角也不宜过大，否则会加速轮胎不规则的磨损，造成浪费。

2.4前轮前束

前轮前束指同轴两端车轮前端面与后端面在车模横向方向的距离差。

前轮前束的作用：由于前轮外倾使轮子滚动类似圆锥滚动，导致车轮向外的趋势，但由于舵机拉杆的存在，车轮不可能向外滚开，则增加了轮胎外侧的磨损。前轮前束可以抵消车轮外倾的副作用。可以通过调节舵机横杆的长度，调整前轮前束。前束值 = 两前轮后端距离 - 两前轮前端距离。根据经验，前束值控制在10mm以内。

2.5编码器安装

能够稳定、精确地得到电机转速值，我们的车模安装了编码器。采用车模原装的固定孔以及齿轮进行固定。为了保证传动部分的流畅，安装时应保证传动齿轮保持平行。同时不要将两齿轮全部重合，避免轮轴之间的摩擦产生的噪音，但这也会造成齿轮的部分磨损。齿轮传动部分安装位置的不恰当，会大大增加电机驱动后轮的负载，左右负载不均匀。会严重影响控制。

2.6舵机安装

舵机有卧式安装和立式安装两种安装方式。 本车模舵机采用立式安装方式，安装好后，安装上舵机摆臂，舵机摆臂应与前轮拉杆保持在同一个竖直平面上,使得舵机施加在拉杆上的拉力、推力在前后方向上的分量尽量小。好的舵机安装方案至关重要，优化舵机的安装位置及其力矩延长杆长度在加快转向结构的响应速度上能起到很大帮助。通过反复实验，综合考虑了速度与力矩的关系，我们确定了摆臂长度的合理范围。

2.7摄像头的安装

摄像头安装主要考虑两方面，一是图像的失真要小，二是前瞻远，同时考虑到主板的放置，因此将摄像头固定在舵机与主板之间。 因为智能视觉组不限高度，可以将摄像头提高以获得更多前瞻，同时为了保证图像的失真要小，经过多次测试后选择将摄像头支架调整到距离地面 25cm 处。

▲ 图 6摄像头的安装方式

 

第三章 电路设计

---

从最初进行硬件电路设计时我们就既定了系统的设计目标：可靠、高效、简洁，在整个系统设计过程中严格按照规范进行。 可靠性是系统设计的第一要求，做好各部分的接地、屏蔽、滤波等工作，考虑到走线问题，设计为 4 个部分得 PCB 电路板，使本系统工作的可靠性达到了设计要求。硬件是基础，只有一个良好、稳定的硬件环境才能保证车能平稳快速的行驶。我们在整个系统设计过程中严格按照规范进行。本着可靠、高效的原则， 在满足各个要求的情况下，尽量使设计的电路简单，PCB 的效果简洁，小车更平稳。

3.1单片机系统总体设计

其原理图如下所示：

▲ 图7 总体原理图

▲ 图8 主板布局

3.2电源管理模块

电源是整个智能车系统动力的来源。稳定的电源是系统正常可靠工作的保证。在设计的时候，需要考虑电源转换效率，降低噪声，防止干扰和电路简单等方面的问题。

本车使用的是智能车比赛组委会提供的 7.2V，2000mAh Ni-Cd 可充电蓄电池，由于各个器件所需的工作电压，电流各不相同，因此要对电源 电压 进行调节。本系统主要包括以下几种不同的工作电压：7.2V 电源电压直接给电机驱动 MOS 供电；经 5 V 稳压模块稳压后核心板，外设电路供电。本车的稳压电源电路含有 5 V 稳压模块，5 V 稳压电路主要用到的芯片是SY8205以及3.3V稳压芯片RT9013。

为满足实际需求，存在四种电压：
1）智能车使用的电池，充满时电压在7.8～8.2V。可直接用于电机和舵机供电。
2）一些数字器件使用直流5V，5V电源选用两个稳压芯片SY8205。一个给外设，核心板供电，一个给舵机电源供电。
3）使用3.3V采用线性稳压芯片RT9013。输入端接SY8205的输出，为防止电机干扰，输出端加磁珠，给核心板供电。
4）电机驱动模块使用直流17V，使用一款5-12V 升压电源模块。

加若干LED显示各类电源工作状况。
电路实际原理图如下所示：

3.3电机驱动模块

驱动电路的基本原理是 H 桥驱动原理，目前流行的 H 桥驱动电路有：H 桥集成电路，如 MC33886；集成半桥电路，如 BTS7970 等；MOS 管搭建的 H 桥电路。我们采用 HIP4082 全桥驱动芯片及四个 MOSFET 组成全桥的电机驱动电路。 电机驱动模块电路如图所示

▲ 图10 驱动电路

▲ 图11 驱动主板布局

3.4 舵机电源模块

比赛提供的舵机为 S3010，该舵机工作电压 4.0V-6.0V，工作时所需电流为 175mA。为了保证舵机正常工作且不影响其他电路工作，为此我们采用线性 电源 LT1764 给舵机供电，该芯片输出电流可达 3A，此时压差为 340mV，输入电压范围广：2.7V-20V，输出电压范围为 1.21V-20V，输出电压与外部配比电阻有关。对于舵机稳压芯片，由于对于电源纹波没有很高要求，就选用SY8205稳压作为舵机稳压芯片，可以通过扭动滑动变阻器来调节输出电压的大小， 方便通过比较不同电压下舵机的工作情况来选取最佳工作电压。

如下图所示电路：

▲ 图12 舵机稳压电路

3.5 其他外设模块

3.5.1无线串口模块

使用逐飞的无线 USB/无线转串口模块套件，实现实时主机与核心板通信，同时也可以实现遥控车无线更改参数等操作，大大节约了时间。

3.5.2 编码器测速模块

本小车使用龙邱科技的 512 线 mini 型编码器进行小车的测速，工作电压在 3.3V- 5V。处理器通过读取编码器脉冲数来实现小车速度的测定，通过读取编码器旋转方向脚的高低电平来检测电机的正反转。

3.5.3 陀螺仪模块

采用ICM-20602六轴加速度传感器，内部集成姿态解算器，配合动态卡尔曼滤 波算法，能准确的得出当前姿态，姿态测量精度静态 0.05 度，动态 0.1 度， 稳定性极高。

 

第四章 控制软件设计

---

高效稳定的软件程序是小车高速平稳运行的基础。在车模的转向和速度控制方面，我们使用了鲁棒性优秀的传统PID控制算法，配合使用理论计算和实际参数补偿的办法，使智能车能够稳定快速完成比赛。车模的控制主要通过对MT9V034摄像头、ICM20602陀螺仪、1024线正交解码编码器、OPENART mini深度学习及其与MIMXRT1064的串口通信等数据的处理，进行方向环舵机的PD 、速度环电机的 PI、激光打靶控制，路径优化处理。

4.1 传感器采集处理算法

4.1.1 摄像头采集处理算法

智能视觉组赛道由白色 PVC 跑道与黑色胶带边沿组成。数字摄像头可直接采集赛道灰度值，并根据灰度值通过大津法进行二值化，灰度值高的为白色，灰度值低的为黑色。因为环境光线不均匀、赛道噪声、畸变的影响下，MT9V034摄像头采集的图像必须经过去噪后才能进一步利用，否则会对车模姿态控制造成很大影响。在排除图像的干扰信息之后，为车模的姿态控制尽可能快速稳定的提供准确的赛道信息。

在去噪之后，我们要采集赛道两边的边缘位置，以及对车库、十字路口、环岛、三岔路等赛道元素的判别。

使用MT9V034摄像头采集赛道信息，为能提前判断到环岛、三岔口元素，分辨率设置为180*60。曝光时间设置为200，帧率设置为50帧。图像增益可以在曝光时间固定的情况下改变图像亮暗程度，通过长时间测试，我们将图像增益设置为32，图像增益过大或过小会导致图像因为环境光不均匀导致过曝或者过暗。

4.1.2 摄像头采集处理算法

摄像头算法主要包括:
(1) 逐行扫描原始图像,大津法二值化提取黑白跳变点。
(2) 赛道宽度限幅，事实上赛道宽度在45cm左右，对应着一个图像宽度范围，在确定的赛道宽度范围内提取赛道边沿，排除宽度过小(即距离车模过远)的赛道或其他干扰。
(3) 根据左右两侧是否丢线，丢线行数等，进行元素判别。

4.2 PID控制算法

PID控制算法是结合比例、积分和微分三种环节于一体的控制算法，它是连续系统中技术最为成熟、应用最为广泛的一种控制算法，该控制算法出现于20世纪30至40年代, 以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。

【通用原理部分，此处省略3000字...】

4.2.3 速度控制

为了达到好的速度控制效果，速度选用增量式PID控制闭环方式。通过闭环反馈使系统的响应时间短,精确度高,控制稳定性高。
设定速度主要由道路与直道的偏差来决定，道路越接近直道，设定速度越高，反之越低。C车模有左右两个电机，可以根据偏差通过电机差速减小舵机压力，但由于急弯偏差可能会过大，导致漂移，需要对其进行限幅。

		MOTOR_Speed_Left =L_Duty*100*Pulse_CM_coe;// //获取左轮速度
		MOTOR_Speed_Right =R_Duty*100*Pulse_CM_coe/2.0;//获取右轮速度
	    CarSpeed=(MOTOR_Speed_Left+MOTOR_Speed_Right)/2;
	    chasu=range_protect(Error,-30,30);// 差速限幅
	    motor_left+=PID_Increase(&LEFTMOTOR_PID,MOTOR_left,(int32)(MOTOR_Speed_Left),(int32)(Speed_set+chasu));//左电机增量式PID控制	
	motor_right+=PID_Increase(&RIGHTMOTOR_PID,MOTOR_right,(int32)(MOTOR_Speed_Right),(int32)Speed_set-chasu);	//右电机增量式PID控制

4.2.4 转向控制

智能车能够在高速情况下，根据各类赛道及时动作，主要取决于对智能车转向的控制。在转弯时，合适的转向角度可以使智能车具有更好的路径，同时也可以提高智能车运行时的稳定性，缩短所需时间。根据实际情况，采用图像计算中线、加权的算法。通过计算权值，对整场图像计算出的有效行进行加权算法处理，对每一行图像乘以一组权值，从而计算出新的赛道中心，使车模路径更优，提高车模完赛的速度与稳定性。

4.3 智能视觉

采用基于OPENART mini的深度学习完成路边目标识别与定位以及三岔路口选择，从而完成智能视觉组的赛道任务。

4.3.1 数据集预处理

由于数据集与OPENART mini所拍摄的图片有很大差距，我们首先对图片进行了高斯模糊、运动处理等操作，对数据集进行加强、扩充。根据比赛规则，标靶外侧有1cm宽紫色框。根据实际测试，该框对定位有很大帮助，但对识别有一定影响。我们采用了将数据集先进行加框处理。

▲ 图14 预处理前的图像

▲ 图15 预处理后的图像

4.3.2 模型训练

本系统的神经网络模型使用开源人工神经网络库Keras进行设计、调试、评估和应用。Keras是一个用python编写的深度学习API，可以在机器学习平台Tensorflow上运行，作为神经网络的推理引擎。

Keras有Sequential API model和Function API model 两种模型，我们采用Sequential API model，它简单，快速，易理解，适合简单的模型，所有的层封装在一起调用model()就可以输出结果。

分类任务本身是一个典型的十分类问题。CNN领域有一些经典的十分类问题，拿最耳熟能详的例子，cifar10和LeNet-5为例，数据集本身的体量很大，每一子类都有上万的可用数据，而本次比赛，每一类仅有约400个。对于一些体量比较大的模型，MCU处理时间会很长，这与快速完成比赛任务相冲突。因此我们综合考虑模型准确性与其所耗时间，将模型的大小限制为4层。最终构建了一个有3Conv+1Dense的CNN，即一个具有3层卷积层与1层全连接层的简单卷积神经网络模型。

参数方面：
(1) 过大的学习率会提高模型的学习速度，但是太大了可能反而适得其反，还可能会导致模型无法得到最优解；而过小的学习率，会导致训练速度过慢，陷入局部最优不能自拔。通过长时间测试，我们选定
opt = Adam(lr=0.0005)
即，学习率为0.0005。

(2) epochs代表总共迭代遍历数据集次数，batch_size为一次迭代需要使用的数据量。batch_size小能让模型迭代快，但是训练效果容易震荡，还妨碍学到全局特征；反之能让学习过程更平稳，但模型收敛效率会下降，还可能陷入局部最优。最终我们设置为
model.fit(x_train,y_train,epochs=100,batch_size=32,validation_data=(x_test,y_test),callbacks=callbacks)
即epochs为100, batch_size为32。

4.3.2 模型量化部署

模型训练完毕后，需要对训练导出的.h5文件进行模型的量化和部署。由于所使用的模型层数较少，所以我们使用性能相对好的NNCU工具。

▲ 图16 NNCU工具

量化完成后，将导出的.nncu文件放置在SD卡根目录，将SD卡插入OPENART mini，即可调用.nnuc文件完成对水果、动物、数字的识别。

4.3.3 激光打靶

根据规则，在识别到图案为水果时，需要使用激光发射器对准靶心发送一束激光。OpenART mini识别到水果后，调整云台舵机使得激光对准水果图片中心，以125Hz、50000占空比开启激光一秒后关闭，完成打靶。

 

第五章 软件开发工具

---

智能车是一个复杂的系统模型，所以在开发中使用合适有效的调试工具进行测试与调试显得尤为重要。

5.1 MDK

程序开发在 MDK-ARM IDE 下进行， MDK-ARM软件为基于Cortex-M、Cortex-R4、ARM7、ARM9处理器设备提供了一个完整的集成开发环境，调试器和仿真环境。 MDK-ARM专为微控制器应用而设计，而且功能强大，能够满足大多数苛刻的嵌入式应用。
MDK-ARM 中包含一个全软件的模拟程序(simulator)。用户不需要任何硬件支持就可以模拟各种 ARM 内核、外部设备甚至中断的软件运行环境。

▲ 图17 MDK界面

5.2 PyCharm

PyCharm是一种Python IDE（Integrated Development Environment，集成开发环境），带有一整套可以帮助用户在使用Python语言开发时提高其效率的工具，比如调试、语法高亮、项目管理、代码跳转、智能提示、自动完成、单元测试、版本控制。
其提供了一个带编码补全，代码片段，支持代码折叠和分割窗口的智能、可配置的编辑器，可帮助用户更快更轻松的完成编码工作。该IDE可帮助用户即时从一个文件导航至另一个，从一个方法至其申明或者用法甚至可以穿过类的层次。若用户学会使用其提供的快捷键的话甚至能更快。用户可使用其编码语法，错误高亮，智能检测以及一键式代码快速补全建议，使得编码更优化。
用户可以用其自带的功能全面的调试器对Python或者Django应用程序以及测试单元进行调整，该调试器带断点，步进，多画面视图，窗口以及评估表达式。

▲ 图18 PyCharm界面

5.3 OpenMV IDE

OpenMV IDE是用来编程OpenMV Cam的工具。它具有一个由QtCreator支持的功能强大的文本编辑器， 一个帧缓冲器查看器，直方图显示器，以及一个用于OpenMV调试输出的集成串行终端。OpenMV IDE 可以轻松地对 OpenMV Cam 进行编程。它成本低易拓展，开发环境友好，除了用于图像处理外，还可以用Python调用其硬件资源，进行I/O控制，与现实世界进行交互。

▲ 图20 OpenMV IDE界面

5.4 Altium Designer

Altium Designer 是软件开发商Altium公司推出的一体化的电子产品开发系统，主要运行在Windows操作系统。这套软件通过把原理图设计、电路仿真、PCB绘制编辑、拓扑逻辑自动布线、信号完整性分析和设计输出等技术的完美融合，为设计者提供了全新的设计解决方案，使设计者可以轻松进行设计，熟练使用这一软件使电路设计的质量和效率大大提高。拓宽了板级设计的传统界面，全面集成了FPGA设计功能和SOPC设计实现功能，允许工程设计人员能将系统设计中的FPGA与PCB设计及嵌入式设计集成在一起。

▲ 图21 Altium Designer界面

5.5 名优科创调试助手

在智能车通过串口或蓝牙与计算机进行交互的过程中，我们使用了名优科创调试助手作为上位机，对串口进行配置，设置好串口通信协议就可以将采集的数据通过串口发送到电脑上，从而得出需要的信息，例如电机占空比、车速、陀螺仪数据等。

▲ 图22 名优科创调试助手界面

 

第六章 车模参数

---

 

第七章 总结

---

自报名参加第十六届全国大学生智能汽车竞赛， 我们组三个人从查找资料、设计机构、组装车模、编写程序一步一步进行，在指导老师、领队的指导下和组员的不断努力配合下，最终完成了比赛要求的任务。

在这段短暂的时间里，我们实践并巩固了各种电路知识，学到了各种各样的算法，了解了接卸结构的搭建。炸过好多块板子，烧过不少核心板，也推翻过很多设计思路。从发现问题到解决问题，一步一步地自己动手动脑去完成一个目的。从车模理论设计、实际制作、整车调试，再到比赛现场调试，每一个环节都需要团队成员一起努力。

在这几个月的备战中，在场地、经费方面都的到了学校和学院的大力支持， 在此特别感谢一直支持和关注智能车比赛的学校和学院领导以及各位老师。同时也感谢比赛组委会能组织这样一项很有意义的比赛。

 

参考文献：

---

[1] 邵贝贝. 单片机嵌入式应用的在线开发方法. 北京：清华大学出版社.2004
[2] 卓晴，黄开胜，邵贝贝．学做智能车．北京：北京航空航天大学出版社.2007
[3] 梅晓榕，自动控制原理（第二版）.北京：科学出版社，2007
[4] 张文春. 汽车理论[M]．北京．机械工业出版社．2005

附录 A：程序源代码

void main(void)
{
    DisableGlobalIRQ();
    board_init();
    //务必保留，本函数用于初始化MPU 时钟
    systick_delay_ms(300);
    //此处编写用户代码(例如：外设初始化代码等)
    Initial();     //总初始化
    //总中断最后开

    while(SysTickTime<1000)
    {
        SysTickTime++;
        ICM20602_GetData(&GYRO, &ACC);
        Data_Filter();//获取姿态解算所需数据
        IMU_update(1.0f,&(sensor.Gyro_deg), &(sensor.Acc_mmss),&imu_data);
    }
    gpio_set(B11,1);
    systick_delay_ms(100);
    gpio_set(B11,0);
    //总中断最后开
    EnableGlobalIRQ(0);
    while (1)
    {
        //ips200_displayimage032(Pixle[0], MT9V03X_CSI_W, MT9V03X_CSI_H);
        ips200_showstr(0,200,"lef");
        ips200_showuint8(30,200,leftstate);
        ips200_showstr(60,200,"rig");
			  ips200_showuint8(90,200,rightstate);
			  ips200_showstr(120,200,"ku");
			  ips200_showint8(150,200,garage_flag);
				ips200_showstr(0,240,"jd");
				ips200_showfloat(30,240,angle_ture_i,3,2);
			

			  //UART_debug();
			
			
//			ips200_showint8(200,200,Openartmini_X);
//			ips200_showint8(0,200,sanmiao_flag);

//			ips200_showint8(0,200,cha_right);
//			ips200_showint8(50,200,sancha_flag);
//			ips200_showint8(100,200,tag_flag);
    }
}
   if(PIT_FLAG_GET(PIT_CH1))
    {
        PIT_FLAG_CLEAR(PIT_CH1);
        ICM20602_GetData(&GYRO, &ACC);
        Data_Filter();//获取姿态解算所需数据
        IMU_update(0.001f,&(sensor.Gyro_deg), &(sensor.Acc_mmss),&imu_data); 		//姿态解算
        //UART_debug();
        PIT2count++;
        if(PIT2count==5)
        {   PIT2count=0;
            //Angle_Calculate();
							if(gpio_get(D4))
		{
			leftchuku();
		}
		else
			{	
						rightchuku();
			}
            // Angle_Calculate2();
        }

    }

    if(PIT_FLAG_GET(PIT_CH2))

    {
        PIT_FLAG_CLEAR(PIT_CH2);

    }

    if(PIT_FLAG_GET(PIT_CH3))
    {	
        PIT_FLAG_CLEAR(PIT_CH3);

        if(!stop_flag&&!sanmiao_flag)
        {
          MOTOR_Control(speed_basic_left,speed_basic_right);
				}
        else
        {
            if(CarSpeed>100)
                MOTOR_Control(-15000,-15000);
            else   MOTOR_Control(0,0);
        }
        if(count_flag)
        {
            count++;
        }
        if(count==5000)
        {
            count_flag=0;
            count=0;
					sanchacount=1;
				banmaxianstate=1;
        }
				if(sanmiao_flag)
				{
					sanmiaocount++;
				}
				if(sanmiaocount==3500)
        {
            sanmiao_flag=0;
            sanmiaocount=0;
        }
    }
    __DSB();
}
void sancha_right()
{

//	chazhi+=3;
//	 Error=-(22*sin(chazhi*3.14/180)+4);
//	smotor_control();
//	angle_i+=GYRO_Real.Z;
//        angle_ture_i= angle_i*0.0045;
//        if (angle_ture_i>=25||angle_ture_i<=-25)////转一圈  30
//        {
//				sanchastate = 0;
//        sancha_flag=0;
//					sanchajishu=1;
//					chazhi=1.0;
//        }
				pwm_duty(PWM4_MODULE2_CHA_C30,8000);
				angle_i+=GYRO_Real.Z;
				angle_ture_i= angle_i*0.090;
        if (angle_ture_i>=25||angle_ture_i<=-25)////转一圈  30
        {
					angle_i=0;
					angle_ture_i=0;
				sanchastate = 0;
        sancha_flag=0;
				sanchajishu=1;
        }

							
}