328 lines
8.2 KiB
C++
328 lines
8.2 KiB
C++
#include<iostream>
|
||
#include<string>
|
||
#include"eyebot++.h"
|
||
#include"maze_func.h"
|
||
#include"maze_parameter.h"
|
||
using namespace std;
|
||
|
||
|
||
/*---检查单元格各方向是否已被到访---*/
|
||
bool check_mark()
|
||
{ bool check = false;
|
||
int x, y;
|
||
x = rob_x; //机器人当前x坐标
|
||
y = rob_y; //机器人当前y坐标
|
||
|
||
bool N_road = ((wall[x][y+1][0] + 1) % 2 && mark[x][y+1]);
|
||
bool W_road = ((wall[x][y][1] + 1) % 2 && mark[x-1][y]);
|
||
bool S_road = ((wall[x][y][0] + 1) % 2 && mark[x][y-1]);
|
||
bool E_road = ((wall[x+1][y][1] + 1) % 2 && mark[x+1][y]);
|
||
|
||
if (x>0 && y>0) //情况1
|
||
{
|
||
if (N_road && W_road && S_road && E_road)
|
||
{
|
||
check = true;
|
||
}
|
||
}
|
||
else if (x==0 && y>0) //情况2
|
||
{
|
||
if (N_road && S_road && E_road)
|
||
{
|
||
check = true;
|
||
}
|
||
}
|
||
else if (x>0 && y==0) //情况3
|
||
{
|
||
if (N_road && W_road && E_road)
|
||
{
|
||
check = true;
|
||
}
|
||
}
|
||
else //情况4
|
||
{
|
||
if (N_road && E_road)
|
||
{
|
||
check = true;
|
||
}
|
||
}
|
||
// if (check)
|
||
// {
|
||
// goto end_explore;
|
||
// }
|
||
return check;
|
||
}
|
||
|
||
/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/
|
||
void maze_entry(int x, int y, int dir, int open)
|
||
{
|
||
int maze_dir = (dir + 4) % 4;
|
||
|
||
if (open) //获取到的open信息为是否有路径信息，转换为墙壁信息
|
||
{
|
||
open = 0;
|
||
}
|
||
else
|
||
{
|
||
open = 1;
|
||
}
|
||
|
||
switch (maze_dir)
|
||
{
|
||
case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
|
||
wall[x][y+1][0] = open;
|
||
break;
|
||
|
||
case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
|
||
wall[x][y][1] = open;
|
||
break;
|
||
|
||
case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
|
||
wall[x][y][0] = open;
|
||
break;
|
||
|
||
case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息
|
||
wall[x+1][y][1] = open;
|
||
break;
|
||
}
|
||
}
|
||
|
||
/*---X坐标更新---*/
|
||
void xneighbor(int x, int dir)
|
||
{
|
||
int maze_dir = (dir + 4) % 4;
|
||
switch (maze_dir)
|
||
{
|
||
case 0:
|
||
break;
|
||
|
||
case 1:
|
||
rob_x = x-1;
|
||
break;
|
||
|
||
case 2:
|
||
break;
|
||
|
||
case 3:
|
||
rob_x = x+1;
|
||
break;
|
||
}
|
||
}
|
||
|
||
/*---Y坐标更新---*/
|
||
void yneighbor(int y, int dir)
|
||
{
|
||
int maze_dir = (dir + 4) % 4;
|
||
switch (maze_dir)
|
||
{
|
||
case 0:
|
||
rob_y = y+1;
|
||
break;
|
||
|
||
case 1:
|
||
break;
|
||
|
||
case 2:
|
||
rob_y = y-1;
|
||
break;
|
||
|
||
case 3:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/*---对PID控制的积分部分I限幅---*/
|
||
void eI_xianfu(int eI_max)
|
||
{
|
||
if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
|
||
{
|
||
err_sum = eI_max;
|
||
}
|
||
}
|
||
|
||
/*---位置式PID算法---*/
|
||
void PID_AL()
|
||
{
|
||
err_sum += err; //积分累计误差值
|
||
eI_xianfu(eI_max); //积分部分限幅
|
||
err_diff = err - err_old; //微分部分误差值
|
||
/*---打印输出PID控制的各项参数1---*/
|
||
// cout << "本次误差:" << err << "\t" ;
|
||
// cout << "上次误差:" << err_old << "\t" ;
|
||
// cout << "误差累计:" << err_sum << "\t" ;
|
||
// cout << "误差变化:" << err_diff << "\t" ;
|
||
/*---打印结束---*/
|
||
err_old = err; //将误差幅值到上一次误差
|
||
|
||
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
|
||
/*---打印输出PID控制的各项参数2---*/
|
||
// cout << "PID输出值:" << Kpid << endl ;
|
||
/*---打印结束---*/
|
||
VWSetSpeed(SPEED,Kpid*Alpha); //控制小车的行驶速度，角速度
|
||
}
|
||
|
||
/*---PID控制下的直线行驶---*/
|
||
void PIDStraight()
|
||
{
|
||
int DIST_move; //定义基础角速度和速度,和移动距离
|
||
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
|
||
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
|
||
SPEED = 200;
|
||
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
|
||
|
||
do
|
||
{
|
||
L_PSD = PSDGet(PSD_LEFT); //读取左侧和墙壁的距离
|
||
R_PSD = PSDGet(PSD_RIGHT); //读取右侧和墙壁的距离
|
||
F_PSD = PSDGet(PSD_FRONT); //读取前方和墙壁的距离
|
||
/*---打印输出PSD数值---*/
|
||
// cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
|
||
/*---打印结束---*/
|
||
|
||
if (100<L_PSD && L_PSD<180 && 100<R_PSD && R_PSD<180) //如果两侧都有墙的情况
|
||
{
|
||
err = L_PSD - R_PSD;
|
||
PID_AL();
|
||
}
|
||
else if (100<L_PSD && L_PSD<180) //只有左侧有墙的情况
|
||
{
|
||
err = L_PSD - DIST_wall;
|
||
PID_AL();
|
||
}
|
||
else if (100<R_PSD && R_PSD<180) //只有右侧有墙的情况
|
||
{
|
||
err = DIST_wall - R_PSD;
|
||
PID_AL();
|
||
}
|
||
else
|
||
{
|
||
L_PSD = L_PSD % DIST_cell;
|
||
R_PSD = R_PSD % DIST_cell;
|
||
err = L_PSD - R_PSD;
|
||
PID_AL();
|
||
}
|
||
VWGetPosition(&x_2, &y_2, &phi_2);
|
||
DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
|
||
if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
|
||
{
|
||
SPEED = SPEED2;
|
||
}
|
||
}while (DIST_move < DIST_cell && F_PSD > DIST_wall); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||
}
|
||
|
||
/*---行驶到指定单元格---*/
|
||
void go_to(int dir)
|
||
{
|
||
int turn; //定义转向倍率
|
||
int maze_dir;
|
||
//static int cur_x,cur_y,cur_p;
|
||
|
||
maze_dir = (dir + 4) % 4; //保证机器人dir方位在[0-3]之间
|
||
turn = maze_dir - rob_dir; //旋转角度倍数
|
||
//turn = turn == +3 ? turn = -1 : turn = turn; 三目运算符？能用吗？
|
||
|
||
if (turn == +3) //确保机器人是旋转90度而不是270度，减少时间
|
||
{
|
||
turn = -1;
|
||
}
|
||
if (turn == -3)
|
||
{
|
||
turn = +1;
|
||
}
|
||
|
||
if (turn) //如果turn不是0，则执行旋转命令
|
||
{
|
||
VWTurn(turn*90,45);
|
||
VWWait();
|
||
}
|
||
PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格
|
||
VWWait(); //等待下一条指令
|
||
|
||
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向
|
||
xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
|
||
yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
|
||
}
|
||
|
||
/*---检查该方向是否已被标记---*/
|
||
bool unmarked(int x, int y, int dir)
|
||
{
|
||
bool check = false;
|
||
int maze_dir = (dir + 4) % 4;
|
||
switch(maze_dir)
|
||
{
|
||
case 0:
|
||
if (mark[x][y+1] == 0)
|
||
{
|
||
check = true;
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
if (mark[x-1][y] == 0)
|
||
{
|
||
check = true;
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
if (mark[x][y-1] == 0)
|
||
{
|
||
check = true;
|
||
}
|
||
break;
|
||
|
||
case 3:
|
||
if (mark[x+1][y] == 0)
|
||
{
|
||
check = true;
|
||
}
|
||
break;
|
||
}
|
||
return check;
|
||
}
|
||
|
||
/*---递归函数---*/
|
||
void explore()
|
||
{
|
||
int F_open,L_open,R_open,old_dir;
|
||
bool check;
|
||
|
||
mark[rob_x][rob_y] = 1; //将当前的单元格设置为已到访
|
||
L_open = PSDGet(PSD_LEFT) > DIST_cell; //获取左侧空间状况
|
||
F_open = PSDGet(PSD_FRONT) > DIST_cell; //获取前侧空间状况
|
||
R_open = PSDGet(PSD_RIGHT) > DIST_cell; //获取右侧空间状况
|
||
|
||
maze_entry(rob_x,rob_y,rob_dir,F_open); //写入前方墙壁信息
|
||
maze_entry(rob_x,rob_y,rob_dir + 1,L_open); //写入左侧墙壁信息
|
||
maze_entry(rob_x,rob_y,rob_dir - 1,R_open); //写入右侧墙壁信息
|
||
check = check_mark(); //检查3个方向是否到访
|
||
if (check)
|
||
{
|
||
goto end_explore;
|
||
}
|
||
|
||
old_dir = rob_dir;
|
||
if (F_open && unmarked(rob_x,rob_y,old_dir)) //如果前方有通路且前方单元格未被探索
|
||
{
|
||
go_to(old_dir); //向前移动一格
|
||
explore(); //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中
|
||
go_to(old_dir + 2); //返回到移动前的单元格
|
||
}
|
||
|
||
if (L_open && unmarked(rob_x,rob_y,old_dir + 1)) //如果左侧有通路且前方单元格未被探索
|
||
{
|
||
go_to(old_dir + 1); //向左移动一格
|
||
explore(); //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中
|
||
go_to(old_dir - 1); //返回到移动前的单元格
|
||
}
|
||
|
||
if (R_open && unmarked(rob_x,rob_y,old_dir - 1)) //如果右侧有通路且前方单元格未被探索
|
||
{
|
||
go_to(old_dir - 1); //向右移动一格
|
||
explore(); //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中
|
||
go_to(old_dir + 1); //返回到移动前的单元格
|
||
}
|
||
end_explore: //check_mark函数确认三个方向都被探索的话，直接跳转到此处结束此次循环
|
||
VWWait();
|
||
} //递归结束
|