求arduino避障小车程序，急！（高额悬赏）

//舵机和超声波调试代码

#include <Servoh>

#include <Metroh>

Metro measureDistance = Metro(50);

Metro sweepServo = Metro(20);

unsigned long actualDistance = 0;

Servo myservo; //创建舵机

int pos = 60;

int sweepFlag = 1;

int URPWM = 3; //PWM输出0－25000us，每50us代表1cm

int URTRIG= 10; // PWM trigger pin PWM串口为10

uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01}; // distance measure command 距离测量命令

void setup(){ // Serial initialization 串行初始化

myservoattach(9); //舵机串口为9

Serialbegin(9600); // Sets the baud rate to 9600

SensorSetup();

}

void loop(){

if(measureDistancecheck() == 1){

actualDistance = MeasureDistance();

// Serialprintln(actualDistance);

// delay(100);

}

if(sweepServocheck() == 1){

servoSweep();

}

}

void SensorSetup(){

pinMode(URTRIG,OUTPUT); // A low pull on pin COMP/TRIG

digitalWrite(URTRIG,HIGH); // Set to HIGH

pinMode(URPWM, INPUT); // Sending Enable PWM mode command 发送使能控制模式命令

for(int i=0;i<4;i++){

Serialwrite(EnPwmCmd[i]);

}

}

int MeasureDistance(){ // a low pull on pin COMP/TRIG triggering a sensor reading 触发传感器读数

digitalWrite(URTRIG, LOW);

digitalWrite(URTRIG, HIGH); // reading Pin PWM will output pulses 读引脚脉宽调制将输出脉冲

unsigned long distance=pulseIn(URPWM,LOW);

if(distance==50000){ // the reading is invalid阅读无效

Serialprint("Invalid");

}else{

distance=distance/50; // every 50us low level stands for 1cm

}

return distance;

}

void servoSweep(){

if(sweepFlag ){

if(pos>=60 && pos<=120){

pos=pos+1; // in steps of 1 degree 1度角度的转动

myservowrite(pos); // tell servo to go to position in variable 'pos' 告诉舵机转动的角度

}

if(pos>119) sweepFlag = false; // assign the variable again 重新分配变量

}else {

if(pos>=60 && pos<=120){

pos=pos-1;

myservowrite(pos);

}

if(pos<61) sweepFlag = true;

}

}

////////////////////////////////////////////////////////////

需要加载一个Metroh的库，这只是调试机器，余下的完全看你的发挥了，加上电机

本程序的前提是将迷宫保存在一个二维数组里，可走的地方为0，不可走的地方为1。由于采用回朔算法，不使用递归，所以首先应该建立一个栈来保存路径，路径是用一个一个点来表示的，也就是说栈中保存的是一系列点的列表。

栈节点类型说明：

struct StackNode

{

POINT Point;

struct StackNode Next, Prev;//双向链表形式

};

栈结构用一个类（CPointStack）实现，声明如下：

class CPointStack

{

public:

void ClearStack();//清空栈

void InitStack();//初始化栈

bool Pop(POINT &pt);//d出顶端元素，并给出该点的坐标，返回是否d出成功

bool Push(POINT pt);//将pt点的信息压入栈，返回是否压入成功

CPointStack();

virtual ~CPointStack();

protected:

StackNode m_psnTop;//栈顶指针

StackNode m_snBottom;//栈底，不保存点信息

};

以下为成员函数实现，都是一些数据结构的 *** 作，应该没什么好说的：

//构造时初始化栈

CPointStack::CPointStack()

{

InitStack();

}

//析构时清空栈

CPointStack::~CPointStack()

{

ClearStack();

}

//将点压入栈(成功返回true,失败返回false)

bool CPointStack::Push(POINT pt)

{

StackNode NewNode = new StackNode;

//是否返回true主要看这里申请内存是否成功

if(!NewNode)

return false;

NewNode->Point = pt;

NewNode->Prev = m_psnTop;

NewNode->Next = NULL;

m_psnTop->Next = NewNode;

m_psnTop = NewNode;

return true;

}

//将点d出栈(成功返回true,栈为空则返回false)

bool CPointStack::Pop(POINT &pt)

{

if(m_psnTop == &m_snBottom)//是否返回true主要看这里栈是否为空

return false;

StackNode p;

p = m_psnTop;

m_psnTop = m_psnTop->Prev;

pt = p->Point;

delete p;

m_psnTop->Next = NULL;

return true;

}

//初始化栈

void CPointStack::InitStack()

{

m_psnTop = &m_snBottom;

m_snBottomNext = NULL;

m_snBottomPrev = NULL;

}

//清空栈

void CPointStack::ClearStack()

{

StackNode p;

while(m_psnTop != &m_snBottom)

{

p = m_psnTop;

m_psnTop = m_psnTop->Prev;

delete p;

}

}

接下来设计怎样走出这个迷宫，也就是迷宫算法的主体部分。再次用一个类实现。

在设计这个类时，我考虑到以下几点：

1、迷宫入口和出口的坐标

2、保存迷宫的2维数组

3、获得路径的函数

但是实际设计时由于没有考虑太多问题，只是以设计迷宫算法和解决一个具体迷宫为目的，所以没有考虑动态大小的迷宫，而是将迷宫大小定为601×401。而且在设计迷宫算法后没有将路径顺序输出而是直接在原图中标识（在保存迷宫数据的表中设置经过的点为2而将死路点设置为3）。

这样迷宫类（CGoMaze）的声明为：

class CGoMaze

{

public:

void Go();//寻找路径的函数

POINT m_ptIn;//入口坐标

POINT m_ptOut;//出口坐标

BYTE m_btArrMaze[401][601];//保存迷宫的二维表

CGoMaze();

virtual ~CGoMaze();

};

最后让我们来看一下CGoMaze::Go()这个函数：

我们模仿人走迷宫时的思路，设置一个当前点，一个目标点（下一个要走的点）。初始情况下当前点为入口，终止条件为当前点为出口，这样，我们的函数大概结构就出来了。

在从入口到出口的过程中程序对当前点的上、下、左、右四个点依次进行判断，当发现任一个方向是未走过的区域时，就将当前点指向那个点进行尝试，同时将当前点入栈并做标记。而当4个方向都不通或已走过时，则为死路，标记当前点为死路并从栈中d出上一个点继续进行尝试，这时因为当前点已被标记为死路，则d出上一个点时就不会重复这条路，达到寻找正确路径的效果。

Go函数的具体实现如下，其实挺简单的^_^：

void CGoMaze::Go()

{

CPointStack psPath;//保存路径的栈

POINT ptCur = m_ptIn, ptNext;//设置当前点为入口

while(ptCurx != m_ptOutx || ptCury != m_ptOuty)//判断是否已经走出

{

ptNext = ptCur;//设置目标点为当前点，便于下面偏移

if(m_btArrMaze[ptCury - 1][ptCurx] == 0)

ptNexty --;//如果上方是空的，则目标点向上偏移

else if(m_btArrMaze[ptCury][ptCurx - 1] == 0)

ptNextx --;//如果左边是空的，则目标点向左偏移

else if(m_btArrMaze[ptCury + 1][ptCurx] == 0)

ptNexty ++;//如果下方是空的，则目标点向下偏移

else if(m_btArrMaze[ptCury][ptCurx + 1] == 0)

ptNextx ++;//如果右边是空的，则目标点向右偏移

else

{//这里是没有路的状态

m_btArrMaze[ptCury][ptCurx] = 3;//标记为死路

psPathPop(ptCur);//d出上一次的点

continue;//继续循环

}

//如果有路的话会执行到这里

m_btArrMaze[ptCury][ptCurx] = 2;//标记当前点为路径上的点

psPathPush(ptCur);//当前点压入栈中

ptCur = ptNext;//移动当前点

}

}

其实这个部分可以将栈设置为CGoMaze类的成员，然后在Go函数里加上：

psPathClearStack();

这样就可以用Timer来演示走迷宫的过程了

#include<stdioh>

#include<stdlibh>

#define M 15

#define N 15

struct mark //定义迷宫内点的坐标类型

{

int x;

int y;

};

struct Element //"恋"栈元素，嘿嘿。。

{

int x,y; //x行,y列

int d; //d下一步的方向

};

typedef struct LStack //链栈

{

Element elem;

struct LStack next;

}PLStack;

/栈函数/

int InitStack(PLStack &S)//构造空栈

{

S=NULL;

return 1;

}

int StackEmpty(PLStack S)//判断栈是否为空

{

if(S==NULL)

return 1;

else

return 0;

}

int Push(PLStack &S, Element e)//压入新数据元素

{

PLStack p;

p=(PLStack)malloc(sizeof(LStack));

p->elem=e;

p->next=S;

S=p;

return 1;

}

int Pop(PLStack &S,Element &e) //栈顶元素出栈

{

PLStack p;

if(!StackEmpty(S))

{

e=S->elem;

p=S;

S=S->next;

free(p);

return 1;

}

else

return 0;

}

/求迷宫路径函数/

void MazePath(struct mark start,struct mark end,int maze[M][N],int diradd[4][2])

{

int i,j,d;int a,b;

Element elem,e;

PLStack S1, S2;

InitStack(S1);

InitStack(S2);

maze[startx][starty]=2; //入口点作上标记

elemx=startx;

elemy=starty;

elemd=-1; //开始为-1

Push(S1,elem);

while(!StackEmpty(S1)) //栈不为空 有路径可走

{

Pop(S1,elem);

i=elemx;

j=elemy;

d=elemd+1; //下一个方向

while(d<4) //试探东南西北各个方向

{

a=i+diradd[d][0];

b=j+diradd[d][1];

if(a==endx && b==endy && maze[a][b]==0) //如果到了出口

{

elemx=i;

elemy=j;

elemd=d;

Push(S1,elem);

elemx=a;

elemy=b;

elemd=886; //方向输出为-1 判断是否到了出口

Push(S1,elem);

printf("\n0=东 1=南 2=西 3=北 886为则走出迷宫\n\n通路为:(行坐标,列坐标,方向)\n");

while(S1) //逆置序列 并输出迷宫路径序列

{

Pop(S1,e);

Push(S2,e);

}

while(S2)

{

Pop(S2,e);

printf("-->(%d,%d,%d)",ex,ey,ed);

}

return; //跳出两层循环，本来用break,但发现出错，exit又会结束程序，选用return还是不错滴

}

if(maze[a][b]==0) //找到可以前进的非出口的点

{

maze[a][b]=2; //标记走过此点

elemx=i;

elemy=j;

elemd=d;

Push(S1,elem); //当前位置入栈

i=a; //下一点转化为当前点

j=b;

d=-1;

}

d++;

}

}

printf("没有找到可以走出此迷宫的路径\n");

}

/建立迷宫/

void initmaze(int maze[M][N])

{

int i,j;

int m,n; //迷宫行,列 [/M]

printf("请输入迷宫的行数 m=");

scanf("%d",&m);

printf("请输入迷宫的列数 n=");

scanf("%d",&n);

printf("\n请输入迷宫的各行各列:\n用空格隔开,0代表路,1代表墙\n",m,n);

for(i=1;i<=m;i++)

for(j=1;j<=n;j++)

scanf("%d",&maze[i][j]);

printf("你建立的迷宫为(最外圈为强)\n");

for(i=0;i<=m+1;i++) //加一圈围墙

{

maze[i][0]=1;

maze[i][n+1]=1;

}

for(j=0;j<=n+1;j++)

{

maze[0][j]=1;

maze[m+1][j]=1;

}

for(i=0;i<=m+1;i++) //输出迷宫

{

for(j=0;j<=n+1;j++)

printf("%d ",maze[i][j]);

printf("\n");

}

}

void main()

{

int sto[M][N];

struct mark start,end; //start,end入口和出口的坐标

int add[4][2]={{0,1},{1,0},{0,-1},{-1,0}};//行增量和列增量 方向依次为东西南北 [/M]

initmaze(sto);//建立迷宫

printf("输入入口的横坐标,纵坐标[逗号隔开]\n");

scanf("%d,%d",&startx,&starty);

printf("输入出口的横坐标,纵坐标[逗号隔开]\n");

scanf("%d,%d",&endx,&endy);

MazePath(start,end,sto,add); //find path

system("PAUSE");

}

在程序的最后加一个条件语句：

若之前pr过程（也就是你的路径输出过程）没被执行过，那么 输出"NO"

当然，在pr过程中要加入一个标记记录这个过程是否被执行了。

还有，你的程序对于题目的输入是不能正常读入的，输出也不符合标准，需要修改。

另外，宽搜尽量用UNTIL语句写，比较好调试。

祝你早日成为编程高手！

以上就是关于求arduino避障小车程序，急！（高额悬赏）全部的内容，包括:求arduino避障小车程序，急！（高额悬赏）、C++设计一个迷宫并走出来、用C语言编写一个迷宫程序，知道出处也行 ~~！等相关内容解答，如果想了解更多相关内容，可以关注我们，你们的支持是我们更新的动力！