mirror of
https://gitee.com/yyz_o/bk_bishe_pi.git
synced 2025-09-07 23:21:26 +00:00
93 lines
1.7 KiB
C++
93 lines
1.7 KiB
C++
#include<iostream>
|
||
#include<string>
|
||
#include"eyebot++.h"
|
||
#include"maze_parameter.h"
|
||
#include"maze_func.h"
|
||
using namespace std;
|
||
|
||
/*---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;
|
||
}
|
||
}
|
||
|
||
/*---行驶到指定单元格---*/
|
||
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 == -2)
|
||
{
|
||
turn = 2;
|
||
}
|
||
|
||
if (turn) //如果turn不是0，则执行旋转命令
|
||
{
|
||
OSWait(2000);
|
||
BOTturn(turn);
|
||
OSWait(2000);
|
||
VWWait();
|
||
}
|
||
PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格
|
||
//~ OSWait(200);
|
||
//~ VWWait(); //等待下一条指令
|
||
MOTORDriveRaw(1,0);
|
||
MOTORDriveRaw(2,0);
|
||
|
||
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向
|
||
xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
|
||
yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
|
||
}
|