电脑小鼠走迷宫(G01组)

项目介绍    

结构与部分驱动

算法部分

成员信息及分工

项目心得及总结

最终源代码

 

 

1.1项目介绍

自动走迷宫的小车是一种由微处理器控制的,集感知、判断、行走功能于一体,由微控制器、前视距离探测器、车轮编码器和驱动机构等组成的一个综合的系统,能够自动寻找最佳路径到达目的地的微型机器人,其中微控制器加上程序就相当于脑袋,前视距离探测器相当于眼睛,驱动机构(车轮编码器)相当于腿。

本次科创我们的小车是采用ARM系列芯片作为控制系统,crosswork为软件开发工具完成各种设计和调试的。

 

    “IEEE标准电脑鼠走迷宫”竞赛是于“第三届上海市嵌入式系统创新设计应用竞赛”同期举办的邀请赛。该竞赛是首次由上海市科学技术委员会、上海市教育委员会和上海市科学技术协会引入的具有国际先进理念的竞赛。

 电脑鼠是一个由微处理器控制的,集感知、判断、行走功能于一体,能够自动寻找最佳路径到达目的地的微型机器人。它能够探索并记忆给定的迷宫,经过处理后得出并沿着一条最佳路径到达目的地。本次大作业对其底层驱动及其接口进行分析。

 本文章中用的库函数为stellaris驱动库函数,该驱动库是以C语言写的,便于理解。

      编译器用的是Crossworks For ARM. CrossWorks是一套完善的专门针对ARM7微处理器而开发的C语言开发系统。它包含ARM GCC C语言编器、CorssWorks C函数库和CrossStudio集成开发环境。使用ARMWNPJ-ARM-20/ WNPJ-ARM-14)的Macgraigor Wiggler或与JTAG接口连接的兼容并行端口,即可以用于开发和调试ARM7ARM9Cortex-M3内核的微处理器。

 

     MicroMouse102 电脑老鼠,采用美国LuminaryMicro 公司生产的32 ARM CortexM3处理器LM3S102,控制和检测红外传感器;主CPU 根据检测到的传感信号,控制电机驱动电路调整行走路径,直到到达终点。

 

LM3S102微控制器包括下列的产品特性:

32RISC性能:

采用为小封装应用方案而优化的 32ARM® Cortex-M3 v7M架构。

提供系统时钟、包括一个简单的24位写清零、递减、自装载计数器,同时具有灵活的控制机制

仅采用与Thumb®兼容的Thumb-2指令集以获取更高的代码密度

工作频率为20-MHz

硬件除法和单周期乘法

集成嵌套向量中断控制器(NVIC),使中断的处理更为简捷

14 中断具有8个优先等级

非对齐式数据访问,使数据能够更为有效的安置到存储器中

精确的位操作(bit-banding),不仅最大限度的利用了存储器空间而且还改良了对外设的控制

 

    内部存储器:

8 KB单周期Flash

• 可由用户管理 对flash块的保护,以2KB为单位

• 可由用户管理对flash的编程

• 可由用户定义和管理的flash保护块

2 KB单周期访问的SRAM

 

通用定时器:

2个通用定时器模块(GPTM),每个提供216-位定时器。 每个 GPTM 可被独立配置进行操作:

• 作为一个32位定时器

• 作为一个32位的实时时钟(RTC)来捕获事件

• 用于脉宽调解器(PWM

32位定时器模式

• 可编程单次触发定时器

• 可编程周期定时器

• 当接入32.768-KHz外部时钟输入时可作为实时时钟使用

• 在调试期间,当控制器发出CPU暂停标志时,在周期和单次触发模式中用户可以使能中止。

16位定时器模式

• 通用定时器功能,并带一个8位的预分频器

• 可编程单次触发定时器

• 可编程周期定时器

• 在调试的时候,当控制器发出CPU暂停标志时,用户可设定暂停周期或者单次模式下的数

16位输入捕获模式

• 提供输入边沿计数捕获功能

• 提供输入边沿时间捕获功能

16PWM模式

• 简单的PWM模式,对PWM信号输出的取反可由软件编程决定

 

GPIO:

高达0-18GPIO,具体数目取决于配置

输入/输出可承受5V

中断产生可编程为边沿触发或电平检测

在读和写操作中通过地址线进行位屏蔽

GPIO端口配置的可编程控制

• 弱上拉或下拉电阻

2mA4mA8mA端口驱动

8-mA驱动的斜率控制

• 开漏使能

• 数字输入使能

                                                                                                                                                                               TOP

 

2.2结构与部分驱动

2.2.1 电源电路

电脑鼠的电源采用4 1.5V 5 号电池供电,电源插座为2.54-2T 型插座。

2.2.2 LDO 和复位电路

LDO 和复位电路采用Sipex 公司性价比高的单芯片解决方案SP6201EM5-3.35V 电源经过SP6201 可以输出3.3V 最大200mA 电流给微处理器和用户外设供电。SP6201 还有使能输入和Reset Not 输出引脚,通过这些引脚实现对微处理器的复位控制。

2.2.3 升压电路

升压芯片采用Sipex公司的低静态电流、高效率的升压芯片SP6641A,输入电压3.3V,输出电压5V

2.2.4 JTAG 接口电路

电脑鼠(MicroMouse102)的JTAG 电路采用ARM 公司提出的标准20 JTAG 仿真

调试接口。

2.2.5 LED 电路

电脑鼠有5个独立的LED,通过LM3S 系统单片机的GPIO 口直接控制,电路采用了I/O 口灌电流的驱动方式来驱动LEDLM3S 系统单片机的灌电流为28mA(可配置),所以不需要驱动就可以点亮LEDGPIO 引脚输出高电平时LED 熄灭,低电平时LED 点亮。

2.2.6 独立按键电路

电脑鼠设计有2 路独立的输入按键,都直接输入到CPU GPIO 输入引脚,当按键未按下时,由于R3R4 上拉电阻的作用,CPU 检测到引脚为高电平;当按

键按下时,CPU 检测到引脚为低电平。

以上的电路图无关痛痒,故不一一列出。

 

2.2.7 电机驱动电路

2.2.7.1电机转动方向的控制

电机采用直流减速电机,最高输出转速为800 /分钟,工作电压为DC3V。电机驱动

电路采用专用的单相直流电动机桥式驱动芯片。

1 左右轮电机电路图

其中LWC1PB5相连,LWC2PA5相连;RWC1PB4相连,RWC2PA4相连.故可以通过给这些管脚写高或低电平来控制轮子的转动方向。具体的程序如下:

API函数解析:

GPIOPinWrite()一次写操作将同时影响请求的管脚(即多个GPIO管脚的状态可以同时改变)第一个参数为GPIO口的基址,第二个为管脚的便宜地址,第三个参数为电平。

2.2.7.2电机速度的控制

    电机的调速采用PWM方式控制,修改脉宽以改变电机的功率。

    两路PWM LM3S102 通用定时器0Timer0)的三种工作模式之一,16 PWM 模式。该模式是将一个32 位的定时器,折分成两个16 位的定时器TimerA TimerB。这些定时器为计数寄存器(GPTMTnR)递减计数,递减到0 时自动加载预装载值(GPTMTnILR)。当然预装载值也是由程序设定,该直也就决定了定时周期,也即PWM 的输出周期。当计数器的值与预装载值相等时,输出PWM 信号有效,当计数器的值与匹配寄存器(GPTMnMATCHR)的值相等时,输出PWM 信号失效。通过软件可以设定PWM 输的信号有效和信号无效的电平状态。当GPTMCTL 寄存器的TnPWML 位值为0 时,信号有效为高电平,信号无效为低电平;TnPWML 位值为1 时,则反之。

GPTM支持简单的PWM生成模式。在该模式中,定时器配置为递减计数器,初值由GPYTMTnILR定义。通过将GPYTMTnIMR寄存器的TnAMS位置1TnCMR位置0TnMR字段设置为0x02来使能PWM模式。

有以上分析可见,PWM模式不同于82536种工作方式,可以非常方便的对脉宽进行调制。

一下为具体的驱动代码:

以下为调速部分:

其中SPEED是图1中的NC脚,当其为高电平时芯片有效,否则无效。通过调节SPEED的占空比来调节车速。

API函数的解析:

GPIODirModeSet():该函数用来设置所选GPIO端口指定的管脚的方向和模式。其中第三个参数可以有GPIO_DIR_MODE_IN(软件控制的输入)、GPIO_DIR_MODE_OUT(软件控制的输出)、GPIO_DIR_MODE_HW(管脚被设置为由硬件控制)。

TimerControlLevel():用于控制PWM的有效电平方向,置低后SetSpeed()中的per参数越小电机转速越快。

TimerMatchSet():通过对per参数的调整可以改变脉宽,从而调整电机的转速。

2.2.8 车速检测电路

车速检测用于检测并记录车体运行的路径,通过车速检测记录车体做迷宫的坐标,同时也起到控制车速和保持左右双轮的速度一致。

检测原理:在左轮和右轮的内则都贴有的光电码盘,码盘由两种颜色组成白色和黑色。红外发射管安装在车轮光电检测码盘的检测区域,当红外发射与接收管正对着黑色边时,红外线没有被反射,接收管的电阻很大;当红外发射与接收管正对着白色边时,红外线被反射,接收管的电阻很小。检测电路如下图所示。

在上图的检测电路中,红外发射与接收管正对着黑色边时,PULSE 输出高电平;正对着白色边时,PULSE输出低电平;从黑色边到白色边,PULSE 输出一个下降沿信号;从白色边到黑色边,PULSE 则输出一个上升沿信号。

LM3S102 单片机可以通过中断操作检测输出脉冲的下降沿信号判断车轮转到的角速度,当检测到13个下降沿信号时,轮子转动了一圈。

LM3S102 单片机的每个GPIO端口的中断能力都由7个一组的寄存器控制。通过这些寄存器可以选择中断源、中断极性以及边沿属性。当一个或大于一个GPIO输入产生中断时,只将一个中断输出发送到供所有GPIO端口使用的中断控制器。对于边沿触发中断,软件必须清楚该中断。对于电平触发中断,假设外部源保持电平不发生变化,以便中断能被控制器识别。由上可知,该中断触发模式极为方便,在监测车速时,不用再添加跟多外设。

初始化程序以及中断服务自程序如下:

中断服务函数:

API函数的解析:

GPIOPinIntStatus():获取所选的GPIO端口的所有管脚的中断状态。返回一个位填充的字节,在这个字节中置位的位同来识别一个有效的屏蔽或原始中断。

2.2.9 红外检测电路

2.2.9.1调制红外信号

采用脉冲调制的反射式红外发射-接收传感器。考虑到环境干扰主要是直流分量,如果采用带有交流分量的调制信号,在平均电流不变的情况下,瞬时电流可以很大,这样也大大提高了信噪比,可以有效避免外界环境变化对系统检测精度的影响。

电路原理图如图2所示,由可调电阻R1,红外线发光管D1 和三极管Q1 构成的电路为红外线发射电路。R1 可以调节红外线发光管的发光强度,Q1 起驱动作用。

2 红外电路原理图

在接收电路中,U1 为一体式红外线接收传感器IRM8601S,,它内部集成自动增益控制电路、带通滤波电路、解码电路及输出驱动电路。但由于它是开漏输出,所以输出端需接一个上拉电阻,见图2中的R3。其中R2 是限流电阻,C1 滤出电源高频干扰。

IRM8601S是一体式红外线接收传感器,它内部集成自动增益控制电路、带通滤波电路、

解码电路及输出驱动电路。当连续收到38KHz 的红外线信号时,将产生脉宽10ms 左右的

低电平。如果没有收到信号,便立即输出高电平。如图2.4 所示,Pulse 为发射控制端,高

电平时发射38KHz 的红外信号。Out 为接收输出端,低电平表示收到信号。

 

2.2.9.2用红外进行挡板检测

Micromouse 中文名为“电脑鼠”,电脑鼠在迷宫中行进时是靠侦测路面情况前进的。

它的左右传感器不但要检测是否存在支路(没有挡板就是一条支路)还要避免和挡板碰触。

因此电脑鼠每一侧在正常情况就需要两组红外传感器,一组检测稍微远一点的距离,判断

是否存在支路,一组检测稍微近一点的距离,判断是否即将碰触挡板。如果是,则要进行微调。这样需要6个传感器来测三个方向的远近距离。

    但是,实际上小车上只装配了3个一体化红外接收器,因此我们采取通过改变发射出的接收传感器能够识别的信号强度,当接收头刚好能接收到信号时,记录下此时发射的强度,这样也就可以大致测算出距离。

    为了改变发射强度,采取改变输出信号的频率,即改变图2PULSE端的频率的方式来达到目的。由于一体化接收头是38KHz 的带通滤波器,所以发射信号的频率偏离38KHz 越多,能检测到的有效信号就越少。这样也就可以改变有效发射信号的强度。

    为了产生调制的红外信号,本设计中采用定时器1 产生38KHz 的调制信号,由PB5 输出,该端口连接到图2中的PULSE 端口。在中断中翻转PB5 输出信号,所以要产生频率为f 的脉冲,定时器的频率要为2f。在本设计中要产生38KHz 的频率,定时器中断频率为76KHz。具体程序如下:

其中TimerConfigure(TIMER1_BASE, TIMER_CFG_32_BIT_PER)是设定TIMER132-位周期定时器模式。TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/76000)为载入初值。载入初值后,计数器开始32位递减计数,到零时自动装载初值并产生一个中断。SysCtlClockGet()为获得当前时钟周期。于是,TIMER1每隔1/76000秒产生一个中断,在中断函数里面把PULSE管脚的电平翻转,这样在PULSE管脚就产生了38KHZ的方波:

API函数的解析:

TimerConfigure():用于配置定时器的模式,定时器模块在配置前前禁能。

参数TIMER_CFG_32_BIT_PER表示定时器配置为32位的单次触发模式,用于调制红外探测信号。

另外,该定时器还可被配置为32位周期触发,32位实时时钟模式,216位的定时器等。可见,改定时器的工作模式有很多,可以满足更多的工作需要。

    在测近距时,只需要更改装入TIMER1的初值,就可以在PULSE脚得到不同频率的信号。近远距检测的程序如下:

------------------------------------------------------------------------------------

// 函数名称: Check_Infrared

// 函数功能: 红外检测函数

// 入口参数: option,选择检测的参数。

//                      0,检测所有参数

//                   1,检测左侧挡板

//                   2,检测前方挡板

//                   3,检测右侧挡板

//                   4,检测左侧安全距离

//                   5,检测右侧安全距离

// 出口参数: State,反应传感器状态。State每四位代表一个参数状态。从左至右20位(0xXXXXX

// 分别表示五个参数:左侧远距,左侧近距,前方,右侧近距,右侧远距。如值为0x11100表示左

// 侧有挡板,且太靠近左侧挡板,前方有挡板,右方没有挡板。

//------------------------------------------------------------------------------------

unsigned long Check_Infrared(unsigned char option)

{

       unsigned long State = 0;

       unsigned char j = 0,i = 0;

       if(option == 4 || option == 5)// 如果只需测近距,直接跳到装载定时器为35KHz的程序处

            goto jinju;

       TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/76000);     // 设置定时器装载值

       if(option==0||option==1)

       {

           Delay(150);

           for(i=0,j=0;i<10;i++)                                // 检测传感器输出信号

           {

               if(GPIOPinRead(GPIO_PORTA_BASE, OUT_L)==0)

                   j++;

           }

           if(j>5)                                                                                         // 左边存在挡板

           {

                     if(option == 1)                                                              // 只测此一个参数

                            return 1;                                                                       // 返回

                     GPIOPinWrite(GPIO_PORTC_BASE, DLL, 0);            // 点亮DLL

                     State |= 1 << 16;                                                                 // 置标志位

           }

           else                                                                                            // 左边存在支路

           {

                     if(option == 1)

                            return 0;

                     GPIOPinWrite(GPIO_PORTC_BASE, DLL, DLL); // 熄灭DLL

                     State |= 0 << 16;

           }

       }

      

       if(option==0||option==3)

       {

           Delay(150);

           for(i=0,j=0;i<10;i++)

           {

               if(GPIOPinRead(GPIO_PORTA_BASE, OUT_R)==0)

                   j++;

           }

           if(j>5)                                                                                         // 右边存在挡板

           {

                     if(option == 3)

                            return 1;

                     GPIOPinWrite(GPIO_PORTB_BASE, DRR, 0);

                     State |= 1 << 0;

           }

           else                                                                                            // 右边存在支路

           {

                     if(option == 3)

                            return 0;

                     GPIOPinWrite(GPIO_PORTB_BASE, DRR, DRR);

                     State |= 0 << 0;

           }

       }

             

TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/71800);     // 设置定时器装载值

       if(option==0||option==2)

       {

           Delay(150);

           for(i=0,j=0;i<10;i++)

           {

               if(GPIOPinRead(GPIO_PORTA_BASE, OUT_F)==0)

                   j++;

           }

           if(j>5)                                                                                         // 前面存在挡板

           {

                     if(option == 2)

                            return 1;

                     GPIOPinWrite(GPIO_PORTC_BASE, DF, 0);

                     State |= 1 << 8;

           }

           else                                                                                            // 前面没有挡板

           {

                     if(option == 2)

                            return 0;

                     GPIOPinWrite(GPIO_PORTC_BASE, DF, DF);

                     State |= 0 << 8;

           }

       }

 

jinju:     

       if(option ==0 || option == 5)

       {

        TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/71500);

 

           Delay(150);

           for(i=0,j=0;i<10;i++)

           {

               if(GPIOPinRead(GPIO_PORTA_BASE, OUT_R)==0)

                   j++;

           }

           if(j>8)                                                                    // 右边太靠近挡板,应向左调整

           {

                     if(option == 5)

                            return 1;

                     GPIOPinWrite(GPIO_PORTC_BASE, DR, 0);

                     State |= 1 << 4;

           }

           else                                                                                            // 右边正常

           {

                     if(option == 5)

                            return 0;

                     GPIOPinWrite(GPIO_PORTC_BASE, DR, DR);

                     State |= 0 << 4;

           }

       }

        TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/71300);

       if(option == 0||option == 4)

       {

           Delay(150);

           for(i=0,j=0;i<10;i++)

           {

               if(GPIOPinRead(GPIO_PORTA_BASE, OUT_L)==0)

                   j++;

           }

 

           if(j>8)                                                                    // 左边太靠近挡板,应向右调整

           {

                     if(option == 4)

                            return 1;

                     GPIOPinWrite(GPIO_PORTC_BASE, DL, 0);

                     State |= 1 << 12;

           }

           else                                                                                     // 左边正常

           {

                     if(option == 4)

                            return 0;

                     GPIOPinWrite(GPIO_PORTC_BASE, DL, DL);

                     State |= 0 << 12;

           }

       }

       return(State);                                                                                    // 返回State

}

 

 

 

 

 

 

 

 

 

 

2.2.10 CPU 及晶振电路

电脑鼠的单片机、晶体振荡器和LDO输出原理如下图所示。该单片机选用LM3S102微处理器。

晶体振荡器标配6.000MHz 的石英晶振,晶振引脚是经过连接器与微处理器连接,可方便地更换晶振。

由于本次所采用的小车并非完全与上面的说明中的连线相同,经过数次失败的调试后终于测得实际的连线为:

                                                                                                                                                                         TOP  

算法部分

            迷宫的规格

迷宫由256个方块(单元)组成,每个方块的大小为18厘米见方,排成16×16列。迷宫的隔墙板沿方块的四周布设,形成迷宫通道。隔墙板高5厘米﹑厚1.2厘米,因此迷宫通道的实际宽度是16.8厘米。隔墙板的两个侧面是白色的,顶部是红色的。迷宫的地板由木质材料做成,涂上没有反光的黑漆。隔墙板的侧面和顶部对红外线有反射特性,而地板则对红外线有吸收特性。竞赛起始点可设在迷宫的任何一角,其三面要有隔墙;竞赛的终点设在迷宫的中央,有四个方块那么大小。图一为一个实际的电脑鼠竞赛用的迷宫照片。

                                                                                                            

图一 电脑鼠竞赛用的迷宫照片

1.2  电脑鼠的规格

电脑鼠要求由参赛者自制,使用电源为能源,不能使用其它可燃物为能源。电脑鼠结构高出隔墙部分的长宽几何尺寸应不大于25×25厘米,对高度没有限制。一个完整的电脑鼠应包含有机身、电源、传感器、微处理器、马达及驱动等部分。电脑鼠的传感器可分为三组,分别用来感知前、左、右三个方向是否已靠近宫壁,并将所获得信息传送给微处理器进行处理。微处理器要完成多种信息的处理,如路径、迷宫地图、行走距离、马达控制等,并要能够作出判断。在马达的控制下,电脑鼠能够完成直行、转弯、掉头以及加减速等动作。通常采用左右独立的马达驱动和控制,以使微处理器的控制更加灵活。图二为一个实际参加竞赛的电脑鼠样例照片。

                                                                      

 

图二  电脑鼠样例照片。

1.3  竞赛的规则

电脑鼠的基本功能是从起点开始走到终点,这个过程称为一次运行,所花费的时间称为运行时间。从终点回到起点所花费的时间不计算在运行时间内。电脑鼠从第一次激活到每次运行开始,所花费的时间称为迷宫时间。如果电脑鼠在比赛时需要手动辅助,这个动作称为碰触。竞赛使用这三个参数,即运行时间﹑迷宫时间和碰触,从速度﹑求解迷宫的效率和电脑鼠可靠性三个方面来进行评分。

电脑鼠的得分是通过计算每次运行的排障时间来衡量的。所谓排障时间是这样计算的:先将迷宫时间的1/30加上一次运行时间,如果这次运行结束以后电脑鼠没有被碰触过,那么还要再减去10秒的奖励时间,这样得到的就是排障时间。电脑鼠在迷宫中的停留或运行的总时间不可超过15分钟,在限时内允许运行多次,允许取其中最短的排障时间作为参赛的计分成绩。当然,排障时间越短越好

例如:一个电脑鼠在迷宫中运行时间为4分钟(240秒)没有碰触过,迷宫时间使用了20秒,这次运行的排障时间就是:20+240×1/30)- 10 = 18秒。

电脑鼠在迷宫通道内行走时不能碰到隔墙板,遇到岔道口时要能够自动作出方向选择。本文规定:如果进入迷宫是为了进行探测和记忆,这次运行就称为试跑;如果进入迷宫是根据先前的记忆和经验,按照智能算法确定最佳路径,并以最快的速度到达目的地,这次运行就称为冲刺

 

2         电脑鼠走迷宫的算法

2.1 探测策略

由于竞赛规则要求电脑鼠在尽可能短的时间内探索并找出一条最佳的路径,而得分不仅仅由迷宫时间决定,运行时间也有一定比重,因此只能对迷宫进行部分探索,从中找出最佳路径。由于电脑鼠在迷宫中遇到岔路时最多只有4种选择,因此可以采取左优先法则、右优先法则、直线优先法则、乱数法则等等。在由起点出发行向目的地的途中,我们选用向心优先法则作为基本探索方案,这种方案在大多数情况下有利于较快速地找到一条到达终点的路径,但也存在部分迷宫使得这种方案的效率反而较为低下;另一方面,在由目的地返回至起点以便开始下一次探索的途中,我们选择左优先法则,以探索尽可能大部分的迷宫,在这一过程中,除向心法则以外的其他法则的平均效果基本相同。

 

电脑鼠走迷宫可以采用全迷宫探索策略,即将迷宫的所有单元均搜索一次,从中找出最佳的行走路径。这种策略需要有足够的时间或探测次数,但在IEEE竞赛规则中每场竞赛只有15分钟的时间,因此是不可能的。另一种方法是部分迷宫探索策略,即在有限的时间或探测次数下,只探测迷宫的一部分,从中找出次最佳的路径,显然只能采用这种策略。

电脑鼠在一巷道内行走,如果最后无路可走,则该巷为死巷。电脑鼠在任一单元内,可能的行走方向最多只有三个(前、左、右),如果有二个或二个以上的可能行走方向,称为交叉,遇有交叉时,由于有多个可以行走的方向,在行走方向的选择上,可有下面的几种选择法则:

Ÿ   右手法则:遇有交叉时,以右边为优先的前进方向,然后是直线方向、左边方向。

Ÿ   左手法则:遇有交叉时,以左边为优先的前进方向,然后是直线方向、右边方向。

Ÿ   中左法则:遇有交叉时,以直线为优先的前进方向,然后是左边方向、右边方向。与此类似的还有中右法则。

Ÿ   乱数法则:遇有交叉时,取随机值作为前进方向。

Ÿ   向心法则:由于终点在迷宫的中心,遇有交叉时,以向迷宫中心的方向为优先的前进方向。

2.2 标记

迷宫及小车记录方法

整个迷宫由一个16×16的二维数组记录,数组中的每个元素是一个struct,数组中的每个元素包含下列信息:

1)该点是否被探测过;

2)围墙的位置;

3)该点是否被判为死点;

再用一个一维数组记录小车的方向,默认初始值记录小车的方向为上,每次转弯或调头时改变相应的数据,根据小车的方向以及所在格子的状态判断小车下一步的动作

 

为了记忆迷宫的详细信息,需要对迷宫单元的位置进行线路标记。全迷宫共有16×16个单元组成,可采用二维坐标方式标记,即用每个单元的XY坐标表示,如起点可标记为(00),终点为(77)。此外,还需要对迷宫单元的可行进方向进行标记,可采用绝对方位或相对方位二种方式。

绝对方位:这是一种与电脑鼠行进方向无关的标记方式,以一个四位的二进制数,分别表示西四个方向。以1表示允许行进(无墙壁),0表示不允许行进(有墙壁)。

相对方位:这是一种与电脑鼠行进方向有关的标记方式,以一个三位的二进制数即可实现标记,分别表示”“”“ 1表示允许(无墙壁),0表示不允许(有墙壁)。

2.3 阻断

在电脑鼠试跑过程中或在最后冲刺时,需要对部分路径进行阻断,即在发现某条路径是死路(只有入口而无出口)时,在该路径的入口处(一般是交叉点)设置标记,即将入口的线路标记由1改为0

2.4 试跑

试跑是获得迷宫地图(各单元路线标记)的唯一方法,因而应在规则允许的情况下,尽可能多的获得迷宫信息,为最后的冲刺准备尽可能多的信息。在试跑过程中,要对经过的单元进行线路标记,同时还要选择一个合适的探测策略。

下面以1/4迷宫为例进行说明。假设迷宫图布局如图三所示,共有8×8=64个单元,起点在左下角(Start),终点在右上角(End)。选用一个8×8的矩阵map保存迷宫地图信息,矩阵的每个元素为1个字节,高4位表示探测到的可行进路径,以绝对方位标记,次序为西。低4位记录自起点的交叉点的个数。探测策略采用右手法则,在初始状态,矩阵map各元素的值均为FFH00H表示死巷。

map2

图三 1/4迷宫

在探测过程中,如果下一个可行进的单元已经探测过(对应的矩阵元素值非00H或非FFH),只有在发现死巷时,才对map中的数据进行修改。对于其它情况,无论探测结果与矩阵中对应元素存储的信息是否一致,均不修改存储的信息。对于复杂的迷宫,往往不能仅使用一种探测策略,而要综合考虑,如增加向心法则。当发现交叉点时,应将该单元坐标和线路特征保存(如入栈),再分析可行的下一个单元是否已经探测过,如果均未探测过,则根据探测策略,选择一方向进行探测。如果部分单元已经探测,则选择未被探测的单元进行探测。遇有死巷,应返回最近的交叉点,同时将死巷阻断,修改入口单元的相应数值。

图四为首次探测时电脑鼠的行走路线示意,电脑鼠在探测过程中,将获得行走过的各单元的线路特征,表一为电脑鼠探测到(50)单元时的二维表(以十六进制表示,高4位为线路标记,低4位为交叉点数)。

map3

图四 首次探测行走路线

7

FFH

FFH

FFH

FFH

FFH

FFH

FFH

end

6

FFH

FFH

FFH

FFH

FFH

FFH

FFH

FFH

5

30H

50H

FFH

FFH

FFH

FFH

FFH

FFH

4

90H

90H

FFH

FFH

FFH

FFH

FFH

FFH

3

90H

D1H

FFH

FFH

FFH

FFH

FFH

FFH

2

90H

90H

FFH

FFH

FFH

FFH

FFH

FFH

1

90H

B2H

FFH

FFH

FFH

FFH

FFH

FFH

0

80H

C0H

60H

60H

60H

60H

FFH

FFH

 

0

1

2

3

4

5

6

7

  

 

 

 

 

 

 

 

 

 

表一 探测到(50)时的map二维表

从图四可以看出,该巷为一死巷,当电脑鼠探测到(70)时,发现是死巷,将按原路返回到最近的交叉点(11),进行阻断,即将向修改为不可行,并修改交叉点的数据,由原值B2H改为90H,死巷中的数据全写零,并继续完成探测,最后得表二 。

7

FFH

FFH

FFH

FFH

FFH

FFH

FFH

end

6

FFH

FFH

FFH

FFH

FFH

FFH

FFH

B0H

5

30H

50H

FFH

FFH

FFH

FFH

FFH

B0H

4

90H

90H

FFH

FFH

FFH

FFH

FFH

90H

3

90H

D1H

FFH

FFH

FFH

FFH

FFH

90H

2

90H

90H

FFH

FFH

FFH

FFH

FFH

A2H

1

90H

C0H

60H

60H

60H

60H

60H

90H

0

80H

00H

00H

00H

00H

00H

00H

00H

 

0

1

2

3

4

5

6

7

 

 

 

 

 

 

 

 

 

 

表二 执行阻断后的二维表

 

 

根据IEEE电脑鼠竞赛规定,当电脑鼠到达终点后,可进行返回探测,从而获得更多的迷宫地图信息。图五为返回探测时的行走路径。在返回探测中,未发现死巷,返回起点,探测后的结果如表三。

 

map4

图五 返回时的探测路径

 

7

50H

60H

60H

60H

60H

60H

30H

end

6

C0H

60H

70H

FFH

FFH

FFH

C0H

B4H

5

30H

50H

D2H

FFH

FFH

FFH

FFH

B3H

4

90H

90H

C0H

60H

30H

FFH

FFH

90H

3

90H

D1H

60H

71H

A0H

FFH

FFH

90H

2

90H

90H

FFH

FFH

FFH

FFH

FFH

A2H

1

90H

C0H

60H

60H

60H

60H

60H

90H

0

80H

00H

00H

00H

00H

00H

00H

00H

 

0

1

2

3

4

5

6

7

 

 

 

 

 

 

 

 

 

 

 

表三 返回探测后的二维表

 

map5

图六 第二次探测路径        

 

第二次探测如图六,在(33)和(25)分别执行阻断,将获得二维表四

 

7

50H

60H

60H

60H

60H

60H

30H

end

6

C0H

60H

70H

72H

60H

30H

C0H

B4H

5

30H

50H

90H

00H

00H

D3H

HHH

B3H

4

90H

90H

C0H

60H

30H

90H

HHH

90H

3

90H

D1H

60H

30H

A0H

90H

HHH

90H

2

90H

90H

HHH

00H

00H

C0H

60H

A2H

1

90H

C0H

60H

60H

60H

60H

60H

90H

0

80H

00H

00H

00H

00H

00H

00H

00H

 

0

1

2

3

4

5

6

7

 

 

 

 

 

 

 

 

 

 

表四 第二次探测后的二维表

2.5 数据补全

由于不可能将所有的单元均探测到,在有了一定的数据基础上,就可以实现数据补全了。数据补全,就是对未探测到的单元,通过周围已有的相数据,可进行补充的一种方法。方法是寻找单元数据为FFH的单元,如果该单元的西四个相邻的单元均为非00HFFH,分析西四个单元的二组数据,看是否有指向该单元的可行方向,如果有,在该方向是相通的,可对数据进行大胆的假设。

对照表四,(65)是未探测过的,其值为FFH,分析与之相邻的(55)(75)和(66)(64)二组数据,(64)的数据为FFH,放弃在南北方向上的补全,考虑东西方向,(55)向东是可行的,(75)向西是可行的,因而可以大胆设想,(65)是东西可行的,数据可设为60H,将60H填入表四,就得到补全后的表五。

7

50H

60H

60H

60H

60H

60H

30H

end

6

C0H

60H

70H

72H

60H

30H

C0H

B4H

5

30H

50H

90H

00H

00H

D3H

60H

B3H

4

90H

90H

C0H

60H

30H

90H

HHH

90H

3

90H

D1H

60H

30H

A0H

90H

HHH

90H

2

90H

90H

HHH

00H

00H

C0H

60H

A2H

1

90H

C0H

60H

60H

60H

60H

60H

90H

0

80H

00H

00H

00H

00H

00H

00H

00H

 

0

1

2

3

4

5

6

7

 

 

 

 

 

 

 

 

 

 

表五 补全后的二维表

2.6 等高表

经过有限次的探测、阻断与补全后,已经可以得到描述迷宫图线路的二维表,虽然不是全部,但已经是部分或大部分,其中可能包含了若干条可以到达终点的路径,为了寻找到达终点的路径,需要制作等高表。等高表是指已探测的各单元距离起点的步数(一个单元为一步),起点的步数为0,表六为通过表五所获得的等高表。等高值以十进制数表示。

 

19

20

21

22

23

24

25

28

18

17

16

17

18

19

26

27

5

6

15

 

 

20

21

22

4

7

14

13

12

21

 

19

3

8

9

10

11

22

 

18

2

9

 

 

 

23

24

17

1

10

11

12

13

14

15

16

0

 

 

 

 

 

 

 

表六 等高表

2.7 可行路径

在等高表中,可行路径上任一单元到起点的步数都是已知的,按从大到小的次序,可以返回起点。按从小到大的次序,可到达终点,这样的可行路径可能不止一个,而是多个。可行路径的查找,从起点开始,在允许前进的方向上,按比当前等高值高1的方向前进,直到终点。有时可能会遇到下一单元的等高值小于当前值(如(62)点)或比当前值高1以上的情况,如果当前单元不是交叉点,可以不予理会,进入下一个单元,按等高值增加的方向查找。如果是交叉点,则要进行趋势分析,找出等高值就增加的方向。舍弃等高值减少的方向。

图七 是根据表六得到的所有可行路径,共有ABCD四条。

map10

图七 所有的可行路径

2.8 可行路径的步数

可行路径的步数,指由起点到达终点,所经过的单元数,可由等高表计算得出。

线路A:由(00)到(74),19步,(74)到(751步,(75)到(761步,(76)到(77),28-27=1步。总计=19+1+1+1=22步。

线路B:由(00)到(62),24步,(62)到(721步,(72)到(74,19-17=2步,(74)到(75),1, 75)到(76),1步。(76)到(77),28-27=1,总计=24+1+2+1+1+1=30步。

同理可得线路C22+1+1=24。线路D28

2.9 最佳路径

电脑鼠要在最短的时间内完成由起点到终点的冲剌,路径的选择至关重要,步数少的路径,是最佳路径的条件之一,但不是唯一条件。考虑电脑鼠在拐弯时,同样需要时间,所以要对拐弯次数加权后加到步数中得到加权步数。

加权步数=步数+拐弯次数×拐弯权重

拐弯次数:一个90度的拐弯算一次,一个180度的拐弯算二次。

拐弯权重:这是一个对结果有重要影响的参数,要结合电脑鼠的结构和试跑确定。如果电脑鼠无加速功能,是以恒速前进,其值可选0.4~1.0之间,如果电脑鼠有变速功能,可根据变速的范围,其最值可适当增加。表7与表8,分别列出了不同的拐弯权重,对加权步数的影响。

线路

步数

拐弯次数

拐弯权重

加权步数

名次

A

22

4

0.5

24

1

B

30

10

0.5

35

4

C

24

10

0.5

29

2

D

28

12

0.5

34

3

表七 拐弯权重为0.5时,各线路的加权步数

 

线路

步数

拐弯次数

拐弯权重

加权步数

名次

A

22

4

1.5

28

1

B

30

10

1.5

45

3

C

24

10

1.5

39

2

D

28

12

1.5

46

4

表八 拐弯权重为1.5时,各线路的加权步数

 

从表七与表八中,已经明显看出了拐弯权重对加权步数的影响,线路B与线路D的排名次序发生了逆转。                          TOP

 

成员信息及分工

 

唐矗:负责模拟算法的实现,以及具体算法的实现。

冷静文:负责硬件精确度等的调试以及驱动的改进。

朱小凡:负责小车硬件各方面和驱动的调试,改进。

刘佳旺:负责小车走迷宫的算法设计,模拟算法以及具体算法的实现。

       

                           朱小凡                                                                唐矗                                                   刘佳旺                                 冷静文

                                                                                                                                                                                                                                           TOP

 

项目心得及总结:

需要改进的地方

我们组的这次科创完成得比较圆满,基本实现了预期的目标,尤其是在迷宫中的转弯,走直线等都非常顺利。但是不可否认,也存在着一些需要改进和加强的地方。

1,硬件上,最初拿到的芯片和所拥有的电路图有些差异,导致对芯片管脚改动较大,以及电动机,红外等的不稳定,使最终车子运行起来会有些不太稳定,电池电量也会影响红外的精确度,以及电机的稳定。可以在车子的稳定性上有所提高,比如更换电机,红外等设备。

   2,软件上,我们组的算法设计还是比较合理的,通过模拟也达到了比较好的效果,但是由于没考虑到芯片的容量问题,结果代码太多,而导致无法烧进程序,最终只能采用了一个简陋的算法,如果时间和条件允许的话,可以在芯片容量上进行扩展,以完善算法,使小车可以适应各种情况。

 

自我评价及感悟,收获:

一个学期的科创三下来还是比较辛苦的,但是最终我们组比较圆满地完成了设计,还是比较让人开心的。

最初拿到小车时非常开心,就开始测试,但是结果发现由于型号不同,原来的管脚无法按说明上的使用,结果导致刚上手就遇到了很大的困难,花费很长时间在管脚的测试上,但是通过这个过程我们也认识到了ARM芯片的一些更深层的结构,也明白了科创三不会是一项简单的工作。

之后虽然纠正了管脚的错误,但是还是遇到了一项严峻的考验。很多程序烧进去后无法正常工作,这个问题困扰了我们将近三周,几乎每天大家都在想到底是为什么,尝试过更换编译器,也曾在实验室连续几个晚上毫无进展。最终我们依靠万用表,一个管脚一个管脚的测试,最终发现了是由于硬件的问题造成的,有两个本来不应该连起来的管脚居然连在一块。找到问题后,我们商量想出了一个好办法,没有在硬件上作相应改动,而是在算法驱动中队管脚的声明和相应代码作了改动。这个寻找错误的过程真的枯燥,但是解决问题的开心还是给了我们很大的鼓舞。

最后在算法移植时发现了芯片容量不够的问题,由于时间有限,只能采用了一个简单的算法,但是大体上还是体现了我们的算法思想,只是有些弱,在一些情况下还不能顺利走出迷宫,这也给我们了一个教训,以后在做一件事情时,一定要全局统筹,全局考虑,不能等到最后才发现某个条件不满足要求,而是要及时发现,及时想办法解决。

                                                                                                                    TOP

 

最终源代码

 

 /****************************************Copyright (c)**************************************************
** Guangzhou ZHIYUAN electronics Co.,LTD.
**
** http://www.embedtools.com
**
**--------------File Info-------------------------------------------------------------------------------
** File Name: Demo.c
** Last modified Date:2007-7-28
** Last Version: v1.0
** Description:
**
**------------------------------------------------------------------------------------------------------
** Created By:
** Created date:
** Version: v1.0
** Descriptions:
**
**------------------------------------------------------------------------------------------------------
** Modified by:
** Modified date:
** Version:
** Description:
**
********************************************************************************************************/
#include "hw_memmap.h"
#include "hw_ints.h"
#include "hw_types.h"
#include "src/gpio.h"
#include "src/sysctl.h"
#include "src/timer.h"
typedef unsigned char uint8; // 无符号8位整型变量
typedef unsigned int uint32; // 无符号32位整型变量

/*PA*/
#define PULSE_R GPIO_PIN_0 // 右侧红外测速脉冲输入
#define OUT_L GPIO_PIN_1 // 左侧红外测挡板信号输入
#define OUT_F GPIO_PIN_2 // 前方红外测挡板信号输入
#define OUT_R GPIO_PIN_3 // 右侧红外测挡板信号输入
#define RWC2 GPIO_PIN_4 // 右侧电机控制信号
#define LWC2 GPIO_PIN_5 // 左侧电机控制信号

/*PB*/
#define RWC1 GPIO_PIN_4 // PWM0,控制右侧电机
#define PULSE_L GPIO_PIN_1 // 左侧红外测速脉冲输入
#define KEY2 GPIO_PIN_3 // 按键
#define SPEED GPIO_PIN_0 // 发射调制红外信号控制
#define PULSE GPIO_PIN_6 // 输出38KHz频率脉冲
#define LWC1 GPIO_PIN_5 // PWM1,控制左侧电机
#define DRR GPIO_PIN_7 // LED,D1

//PC
#define DLL GPIO_PIN_3 // LED,D7
#define DL GPIO_PIN_0 // LED,D5
#define DF GPIO_PIN_1 // LED,D4
#define DR GPIO_PIN_2 // LED,D2
#define STEP 14

uint32 LeftPulse = 0,RightPulse = 0; // 设定运行目标脉冲
uint32 PulCount_L = 0,PulCount_R = 0; // 中断脉冲计数
uint8 WheelStop_L = 0,WheelStop_R = 0; // 电机运行/停止状态
int count_L=0,count_R=0,sign_L=0,sign_R=0;


//------------------------------------------------------------------------------------
// 函数名称:Delay
// 函数功能:延时数量为 d 个指令周期。
// 输入参数:d,将要延时的时间数。
//------------------------------------------------------------------------------------
void Delay(unsigned long d)
{
for(;d;d--);
}

//------------------------------------------------------------------------------------
// 函数名称: Timer1A_ISR
// 函数功能: 定时器1中断处理程序。翻转PULSE。
// 全局变量: PULSE,产生驱动红外的调制信号。
//------------------------------------------------------------------------------------


void Timer1A_ISR(void)
{

TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT); // 清除定时器1中断

GPIOPinWrite(GPIO_PORTB_BASE, PULSE,GPIOPinRead(GPIO_PORTB_BASE, PULSE) ^ PULSE);
// 翻转GPIO B5 端口
}
//------------------------------------------------------------------------------------
// 函数名称: LeftWheelRun
// 函数功能: 左轮控制函数。
// 输入参数: sel,选择轮子的控制方式:0为停止,1为轮子向前,2为轮子向后。
// percen,占空比,其最大值为99,最小值为1,对于轮子的停止控制该参数无效。
//------------------------------------------------------------------------------------
void LeftWheelRun(int sel)
{
switch(sel)
{
//轮子停止转动
case 0:
GPIOPinWrite(GPIO_PORTB_BASE,LWC1,0xff);
GPIOPinWrite(GPIO_PORTA_BASE,LWC2,0xff);
break;
//左轮向后
case 1:
GPIOPinWrite(GPIO_PORTB_BASE,LWC1,0xff);
GPIOPinWrite(GPIO_PORTA_BASE,LWC2,0x00);
break;
//左轮向前
case 2:
GPIOPinWrite(GPIO_PORTB_BASE,LWC1,0x00);
GPIOPinWrite(GPIO_PORTA_BASE,LWC2,0xff);
break;
}
}
//------------------------------------------------------------------------------------
// 函数名称: RightWheelRun
// 函数功能: 右轮控制函数。
// 输入参数: sel,选择轮子的控制方式:0为停止,1为轮子向前,2为轮子向后。
// percen,占空比,其最大值为99,最小值为1,对于轮子的停止控制该参数无效。
// 全局变量: g_Run.WheelStop,标记当前状态。
//------------------------------------------------------------------------------------
void RightWheelRun(int sel)
{
switch(sel)
{
//轮子停止转动
case 0:
GPIOPinWrite(GPIO_PORTB_BASE,RWC1,0xff);
GPIOPinWrite(GPIO_PORTA_BASE,RWC2,0xff);
break;
//右轮向后
case 1:
GPIOPinWrite(GPIO_PORTB_BASE,RWC1,0x00);
GPIOPinWrite(GPIO_PORTA_BASE,RWC2,0xff);
//flagr=1;
break;
//右轮向前
case 2:
GPIOPinWrite(GPIO_PORTB_BASE,RWC1,0xff);
GPIOPinWrite(GPIO_PORTA_BASE,RWC2,0x00);
//flagr=1;
break;

}
}

//------------------------------------------------------------------------------------
// 函数名称: GPIO_Port_A_ISR
// 函数功能: 右轮检测脉冲中断处函数
//
//------------------------------------------------------------------------------------
void GPIO_Port_A_ISR(void)
{
unsigned char IntStatus;
IntStatus = GPIOPinIntStatus(GPIO_PORTA_BASE,true); // 读PA口中断状态
if(IntStatus&PULSE_R) // 是否为左轮脉冲中断
{
if(sign_R)
{
++count_R;
}
else
{
count_R=0;
}
PulCount_R++; // 脉冲计数
if(PulCount_R >= RightPulse) // 判断是否达到设定值
{
RightWheelRun(0); // 停止电机
WheelStop_R= 1; // 置电机停止标志
}
GPIOPinIntClear(GPIO_PORTA_BASE,PULSE_R); // 清中断
}
}
//------------------------------------------------------------------------------------
// 函数名称: GPIO_Port_B_ISR
// 函数功能: 左轮检测脉冲中断处函数
//
//------------------------------------------------------------------------------------
void GPIO_Port_B_ISR(void)
{
unsigned char IntStatus;
IntStatus = GPIOPinIntStatus(GPIO_PORTB_BASE,true); // 读PA口中断状态
if(IntStatus&PULSE_L) // 是否为右轮脉冲中断
{
if(sign_L)
{
++count_L;
}
else
{
count_L=0;
}

PulCount_L++;
if(PulCount_L>= LeftPulse) // 判断是否达到设定值
{
WheelStop_L= 1; // 置电机停止标志
LeftWheelRun(0); // 停止电机

}
GPIOPinIntClear(GPIO_PORTB_BASE,PULSE_L); // 清中断
}
}

void onestepback(int left,int right)
{
WheelStop_L=0;
WheelStop_R=0;
PulCount_L = 0;
PulCount_R = 0;
LeftPulse = left;
RightPulse = right;
LeftWheelRun(1);
RightWheelRun(1);
while(!(WheelStop_L && WheelStop_R)) {}
WheelStop_L=0;
WheelStop_R=0;

}

void stop()
{
WheelStop_L=0;
WheelStop_R=0;
PulCount_L = 0;
PulCount_R = 0;
onestepback(1,1);
LeftWheelRun(0);
RightWheelRun(0);
// Delay(15000);
WheelStop_L=1;
WheelStop_R=1;
// while(!(WheelStop_L && WheelStop_R)) {}
}
void onestep(int left,int right)
{
WheelStop_L=0;
WheelStop_R=0;
PulCount_L = 0;
PulCount_R = 0;
LeftPulse = left;
RightPulse = right;
LeftWheelRun(2);
RightWheelRun(2);
while(!(WheelStop_L && WheelStop_R)){}
WheelStop_L=0;
WheelStop_R=0;
Delay(1100);
}



void turnright(int right,int left)
{
WheelStop_L=0;
WheelStop_R=0;
PulCount_L = 0;
PulCount_R = 0;
LeftPulse = left;
RightPulse = right;
LeftWheelRun(2);
RightWheelRun(1);
while(!(WheelStop_L && WheelStop_R)) {}
WheelStop_L=0;
WheelStop_R=0;
count_R=0;
count_L=0;
}

void turnleft(int right,int left)
{
LeftWheelRun(1);
RightWheelRun(2);
WheelStop_L=0;
WheelStop_R=0;
PulCount_L = 0;
PulCount_R = 0;
LeftPulse = left;
RightPulse = right;
while(!(WheelStop_L && WheelStop_R)) {}
WheelStop_L=0;
WheelStop_R=0;
count_L=0;
count_R=0;

}



void turnback()
{
int j=0;
LeftWheelRun(0);
RightWheelRun(0);
Delay(200);
for(j=0;j<11;j++)
{
turnleft(1,1);
Delay(1000);
}
LeftWheelRun(1);
RightWheelRun(2);
Delay(200);
LeftWheelRun(0);
RightWheelRun(0);
}
//------------------------------------------------------------------------------------
// 函数名称: PWMTimer0AIni
// 函数功能: 定时器PWM初始化。
//
//------------------------------------------------------------------------------------
void PWMTimer0AIni(void)
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0); // 使能定时器0模块
GPIODirModeSet(GPIO_PORTB_BASE, LWC1|RWC1|SPEED , GPIO_DIR_MODE_OUT);

/* 控制引脚输出*/
GPIOPinWrite(GPIO_PORTB_BASE, LWC1|RWC1|SPEED , 0xff); // GPIO输出高电平
GPIODirModeSet(GPIO_PORTA_BASE, LWC2|RWC2 , GPIO_DIR_MODE_OUT); // 控制引脚输出
GPIOPinWrite(GPIO_PORTA_BASE, LWC2|RWC2 , 0xff); // GPIO输出高电平

/* 定时器配置*/
TimerConfigure(TIMER0_BASE,TIMER_CFG_16_BIT_PAIR
|TIMER_CFG_A_PWM|TIMER_CFG_B_PWM); // 16位PWM输出
TimerControlLevel(TIMER0_BASE,TIMER_A,false); // PWM初始化有效信号为低电平
TimerControlLevel(TIMER0_BASE,TIMER_B,false);
TimerLoadSet(TIMER0_BASE,TIMER_A,60000); // 设定PWM频率
TimerLoadSet(TIMER0_BASE,TIMER_B,60000);
}
//------------------------------------------------------------------------------------
// 函数名称: PULSEIni
// 函数功能: 红外调制信号初始化。
//
//------------------------------------------------------------------------------------
void PULSEIni(void)
{
GPIODirModeSet(GPIO_PORTB_BASE, PULSE, GPIO_DIR_MODE_OUT); // 设置 GPIO 为输出口
GPIOPinWrite( GPIO_PORTB_BASE, PULSE,0); // 红外线初始时停止发射

SysCtlPeripheralEnable( SYSCTL_PERIPH_TIMER1 ); // 使能定时器1外设
TimerConfigure(TIMER1_BASE, TIMER_CFG_32_BIT_PER); // 设置定时器1为周期触发模式

TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/76000); // 设置定时器装载值


TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);

TimerEnable(TIMER1_BASE, TIMER_A);
IntEnable(INT_TIMER1A);

}
//------------------------------------------------------------------------------------
// 函数名称: WheelPulseIni
// 函数功能: 轮子脉冲检测初始化。
//
//------------------------------------------------------------------------------------

void WheelPulseIni(void)
{
// 配置引脚为输入
GPIODirModeSet(GPIO_PORTA_BASE, PULSE_R, GPIO_DIR_MODE_IN);
GPIODirModeSet(GPIO_PORTB_BASE, PULSE_L, GPIO_DIR_MODE_IN);
// 配置引脚下降沿触发中断
GPIOIntTypeSet(GPIO_PORTA_BASE,PULSE_R,GPIO_FALLING_EDGE);
GPIOIntTypeSet(GPIO_PORTB_BASE,PULSE_L,GPIO_FALLING_EDGE);
// 使能引脚输入中断
GPIOPinIntEnable(GPIO_PORTA_BASE,PULSE_R);
GPIOPinIntEnable(GPIO_PORTB_BASE,PULSE_L);
// 使能GPIO PA口和GPIO PB口中断
IntEnable(INT_GPIOA);
IntEnable(INT_GPIOB);
}

//------------------------------------------------------------------------------------
// 函数名称: Check_Infrared
// 函数功能: 红外检测函数
// 入口参数: option,选择检测的参数。
// 0,检测所有参数
// 1,检测左侧挡板
// 2,检测前方挡板
// 3,检测右侧挡板
// 4,检测左侧安全距离
// 5,检测右侧安全距离
// 出口参数: State,反应传感器状态。State每四位代表一个参数状态。从左至右20位(0xXXXXX)
// 分别表示五个参数:左侧远距,左侧近距,前方,右侧近距,右侧远距。如值为0x11100表示左
// 侧有挡板,且太靠近左侧挡板,前方有挡板,右方没有挡板。
//------------------------------------------------------------------------------------
unsigned long Check_Infrared(unsigned char option)
{
unsigned long State = 0;
unsigned char j = 0,i = 0;
if(option == 4 || option == 5)// 如果只需测近距,直接跳到装载定时器为35KHz的程序处
goto jinju;
TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/76000); // 设置定时器装载值
if(option==0||option==1)
{
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , SEND); // 发送调制信号
Delay(150);
for(i=0,j=0;i<10;i++) // 检测传感器输出信号
{
if(GPIOPinRead(GPIO_PORTA_BASE, OUT_L)==0)
j++;
}
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , ~SEND); // 停止发送

if(j>5) // 左边存在挡板
{
if(option == 1) // 只测此一个参数
return 1; // 返回
GPIOPinWrite(GPIO_PORTC_BASE, DLL, 0); // 点亮DLL
State |= 1 << 16; // 置标志位
}
else // 左边存在支路
{
if(option == 1)
return 0;
GPIOPinWrite(GPIO_PORTC_BASE, DLL, DLL); // 熄灭DLL
State |= 0 << 16;
}
}

if(option==0||option==3)
{
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , SEND);
Delay(150);
for(i=0,j=0;i<10;i++)
{
if(GPIOPinRead(GPIO_PORTA_BASE, OUT_R)==0)
j++;
}
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , ~SEND);
if(j>5) // 右边存在挡板
{
if(option == 3)
return 1;
GPIOPinWrite(GPIO_PORTB_BASE, DRR, 0);
State |= 1 << 0;
}
else // 右边存在支路
{
if(option == 3)
return 0;
GPIOPinWrite(GPIO_PORTB_BASE, DRR, DRR);
State |= 0 << 0;
}
}

TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/71800); // 设置定时器装载值
if(option==0||option==2)
{
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , SEND);
Delay(150);
for(i=0,j=0;i<10;i++)
{
if(GPIOPinRead(GPIO_PORTA_BASE, OUT_F)==0)
j++;
}
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , ~SEND);
if(j>5) // 前面存在挡板
{
if(option == 2)
return 1;
GPIOPinWrite(GPIO_PORTC_BASE, DF, 0);
State |= 1 << 8;
}
else // 前面没有挡板
{
if(option == 2)
return 0;
GPIOPinWrite(GPIO_PORTC_BASE, DF, DF);
State |= 0 << 8;
}
}

jinju:



if(option ==0 || option == 5)
{
TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/71500);
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , SEND);
Delay(150);
for(i=0,j=0;i<10;i++)
{
if(GPIOPinRead(GPIO_PORTA_BASE, OUT_R)==0)
j++;
}
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , ~SEND);
if(j>8) // 右边太靠近挡板,应向左调整
{
if(option == 5)
return 1;
GPIOPinWrite(GPIO_PORTC_BASE, DR, 0);
State |= 1 << 4;
}
else // 右边正常
{
if(option == 5)
return 0;
GPIOPinWrite(GPIO_PORTC_BASE, DR, DR);
State |= 0 << 4;
}
}
TimerLoadSet(TIMER1_BASE, TIMER_A, SysCtlClockGet()/71300);
if(option == 0||option == 4)
{

// GPIOPinWrite( GPIO_PORTB_BASE,SEND , SEND);
Delay(150);
for(i=0,j=0;i<10;i++)
{
if(GPIOPinRead(GPIO_PORTA_BASE, OUT_L)==0)
j++;
}
// GPIOPinWrite( GPIO_PORTB_BASE,SEND , ~SEND);
if(j>8) // 左边太靠近挡板,应向右调整
{
if(option == 4)
return 1;
GPIOPinWrite(GPIO_PORTC_BASE, DL, 0);
State |= 1 << 12;
}
else // 左边正常
{
if(option == 4)
return 0;
GPIOPinWrite(GPIO_PORTC_BASE, DL, DL);
State |= 0 << 12;
}
}
return(State); // 返回State
}


void Set_Speed(int per)
{
TimerControlLevel(TIMER0_BASE,TIMER_A,false); // PWM有效电平方向
GPIODirModeSet(GPIO_PORTB_BASE, SPEED, GPIO_DIR_MODE_HW); // 禁止GPIO引脚输出,改为PWM输出
TimerMatchSet(TIMER0_BASE,TIMER_A,per*600); // 设置占空比
TimerEnable(TIMER0_BASE,TIMER_A);
}


void rightadjust()
{

LeftPulse = 10; // 设定电机运行任务
RightPulse = 10;
WheelStop_L = 0; // 清零电机停止标志位
WheelStop_R = 0;
LeftWheelRun(0);
RightWheelRun(0);
// Delay(5000);
LeftWheelRun(1);
RightWheelRun(2);
Delay(2000);
// while(Check_Infrared(0)&0x00010);
LeftWheelRun(0);
RightWheelRun(0);
}

void leftadjust()
{
LeftPulse = 10; // 设定电机运行任务
RightPulse = 10;
WheelStop_L = 0; // 清零电机停止标志位
WheelStop_R = 0;
LeftWheelRun(0);
RightWheelRun(0);
LeftWheelRun(2);
RightWheelRun(1);
Delay(1500);
LeftWheelRun(0);
RightWheelRun(0);
}

void leftturn()
{
unsigned long tempstate1=0x00;
unsigned long laststate1=0x00;
int i=0;
while(tempstate1&0x00100)
{
tempstate1 = Check_Infrared(0);
turnleft(1,1);
}

for(i=0;i<6;i++)
{turnleft(1,1);
Delay(1000);
}
LeftWheelRun(0);
RightWheelRun(0);
Delay(1000);

tempstate1 = Check_Infrared(0);
laststate1=tempstate1;
}
//------------------------------------------------------------------------------------
//
//
//------------------------------------------------------------------------------------
int main(void)
{
unsigned long tempstate=0x00;
unsigned long laststate=0x00;
int ding =0;
int UNDONE=0,i=0;
int direction = 1; //记录小车的方向,1,2,3,4分别对应上右下左
// mouse[0][0].indirection = 2;
SysCtlClockSet( SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_6MHZ ); // 设定晶振为时钟源


SysCtlPeripheralEnable( SYSCTL_PERIPH_GPIOA ); // 使能GPIO A口外设
SysCtlPeripheralEnable( SYSCTL_PERIPH_GPIOB ); // 使能GPIO B口外设
SysCtlPeripheralEnable( SYSCTL_PERIPH_GPIOC ); // 使能GPIO C口外设

GPIODirModeSet(GPIO_PORTB_BASE, KEY2, GPIO_DIR_MODE_IN);// 设定按键为输入
if(GPIOPinRead(GPIO_PORTB_BASE, KEY2)==0) // 是否恢复JTAG功能判断
{
while(1);
}
/* 设定JTAG口为GPIO口 */
GPIODirModeSet(GPIO_PORTC_BASE, DLL | DL | DF | DR, GPIO_DIR_MODE_OUT);
GPIODirModeSet(GPIO_PORTB_BASE, DRR , GPIO_DIR_MODE_OUT);
/* 设定测挡板的红外信号端口为输入 */
GPIODirModeSet(GPIO_PORTA_BASE, OUT_L | OUT_R | OUT_F, GPIO_DIR_MODE_IN);
PWMTimer0AIni(); // PWM初始化
PULSEIni(); // 调制信号初始化
WheelPulseIni();
Set_Speed(50);


while(1)
{
LeftPulse = 10; // 设定电机运行任务
RightPulse = 10;
WheelStop_L = 0; // 清零电机停止标志位
WheelStop_R = 0;

onestep(1,1);

tempstate = Check_Infrared(0);
if(!(tempstate&0x01000) && !(tempstate&0x00001))
{
if(count_L>=5)
rightadjust();
count_L=0;
sign_L=1;
}
else
{
count_L=0;
sign_L=0;
}

if(!(tempstate&0x00010) && !(tempstate&0x10000))
{
if(count_R>=5)
leftadjust();
count_R=0;
sign_R=1;
}
else
{
count_R=0;
sign_R=0;
}
if(((laststate&0x10000)&&!(tempstate&0x10000)) || ((laststate&0x00001)&&!(tempstate&0x00001)))
{
for(i=0;i<7;i++)
{onestep(1,1);
Delay(1000);
}
LeftWheelRun(0);
RightWheelRun(0);
Delay(1000);
}
tempstate = Check_Infrared(0);
laststate=tempstate;
if ((tempstate&0x10101)==0x10001)
{
goto end;

}
if ((tempstate&0x10101)== 0x10101)
{
if(direction == 1)
{
turnback();
direction = 3;
goto end;

}
if(direction == 3)
{
turnback();
direction = 1;
goto end;


}
else if(direction == 4)
{
turnback();
direction = 2;
goto end;

}
if(direction == 2 )
{
turnback();
direction = 4;
goto end;
}

}

if ((tempstate&0x10101)==0x10000)
{
if(direction == 1)
{
goto end;
}
if (direction == 3)
{
goto end;
}
if (direction == 2)
{
goto end;
}
if(direction == 4)
{
turnright(5,5);
direction = 1;
goto end;

}
}

if ((tempstate&0x10101)==0x10100)
{
if(direction == 1)
{

turnright(5,5);
direction = 2;
goto end;

}
if (direction == 4)
{
turnright(5,5);
direction = 1;
goto end;

}
if(direction == 3 )
{
turnright(5,5);
direction =4;
goto end;

}
if( direction == 2)
{
turnright(5,5);
direction = 3;
goto end;

}
}

if ((tempstate&0x10101)==0x00101)
{
if(direction == 1)
{

turnleft(5,5);
direction = 4;
goto end;

}
if (direction == 4)
{
turnleft(5,5);
direction = 3;
goto end;

}
if(direction == 3)
{
turnleft(5,5);
direction = 2;
goto end;

}
if(direction == 2)
{
turnleft(5,5);
direction = 1;
goto end;

}
}

if ((tempstate&0x10101)==0x00001)
{
if(direction == 1)
{
goto end;
}
if (direction == 3)
{
turnleft(5,5);
direction = 2;
goto end;

}
if (direction == 4)
{
goto end;
}
if(direction == 2)
{
turnleft(5,5);
direction = 1;
goto end;

}
}

if ((tempstate&0x10101)==0x00100)
{
if(direction == 1)
{

turnright(5,5);
direction = 2;
goto end;

}
if (direction == 2)
{
turnleft(5,5);
direction = 1;
goto end;

}
if (direction == 4)
{
turnright(5,5);
direction = 1;
goto end;

}
if(direction == 3)
{
turnleft(5,5);
direction = 2;
goto end;

}
}

if ((tempstate&0x10101)==0x00000)
{
if(direction == 1)
{
goto end;
}
if (direction == 3)
{
turnleft(5,5);
direction = 2;
goto end;

}
if (direction == 4)
{
turnright(5,5);
direction = 1;
goto end;

}
if (direction == 2)
{
goto end;
}

}
end:
{}

if(tempstate)
{
UNDONE = 1;


if(UNDONE &&(tempstate&0x00010))
{
rightadjust();
UNDONE = 0;
}
if(UNDONE &&(tempstate&0x01000))
{
leftadjust();
UNDONE = 0;

}



}


// 等待过程中进行红外检测
PulCount_L -= LeftPulse; // 误差补偿到下一次运动中
PulCount_R -= RightPulse;
}
}

                                                                                                                                                                         TOP