42 lines
1.6 KiB
C++
42 lines
1.6 KiB
C++
#include<iostream>
|
||
#include<string>
|
||
#include"eyebot++.h"
|
||
#include"maze_func.h"
|
||
#include"maze_parameter.h"
|
||
using namespace std;
|
||
|
||
|
||
/*---定义全局常量---*/
|
||
const int DIST_cell = 360 ;
|
||
const int DIST_wall = 130 ;
|
||
// const int mazesize_x = 6 ;
|
||
// const int mazesize_y = 6 ;
|
||
/*---全局各种常量定义完成---*/
|
||
|
||
/*---定义全局变量---*/
|
||
/*---数组类---*/
|
||
int mark[6][6] = {}; //1=为已访问
|
||
int wall[7][7][2] = {}; //1=墙壁；0=通路
|
||
int map[6][6] = {}; //最短路径求解地图
|
||
int copy_map[6][6] = {}; //copy复制地图
|
||
int path[6][6] = {}; //最短路径结果地图
|
||
// int mark[mazesize_x][mazesize_y] = {}; //1=为已访问
|
||
// int wall[mazesize_x + 1][mazesize_y + 1][2] = {}; //1=墙壁；0=通路
|
||
//wall[1][1][0] = 1; //定义迷宫的初始小车背后为墙
|
||
// int map[mazesize_x][mazesize_y]; //最短路径求解地图
|
||
// int copy_map[mazesize_x][mazesize_y]; //copy复制地图
|
||
// int path[mazesize_x][mazesize_y]; //最短路径结果地图
|
||
/*---机器人坐标---*/
|
||
int rob_x = 1 ; //机械人初始的X坐标
|
||
int rob_y = 1 ; //机器人初始的Y坐标
|
||
int rob_dir = 0 ; //机器人初始的朝向
|
||
|
||
/*---递归算法---*/
|
||
/*---PID算法---*/
|
||
const float Kp = 1, Ki = 0, Kd = 0.5; //定义PID的参数
|
||
float Kpid; //定义PID计算后输出的倍率
|
||
const int Alpha = 2, SPEED = 200; //定义基本角速度和速度
|
||
const int SPEED2 = 0.25*SPEED; //快到目的地时减速
|
||
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||
const int eI_max = 20; //定义积分限幅
|
||
/*---全局各种变量定义完成---*/ |