#include<iostream>
#include<string>
#include<vector>
#include"eyebot++.h"
#include"maze_parameter.h"
// #include"maze_func.h"
using namespace std;

/*---定义数据---*/
/*---定义全局常量---*/
const int DIST_cell = 240;     	//单元格长度
const int DIST_move = 330;		//移动的单元格距离
const int DIST_wall_F = 95;     //与墙壁的距离
//~ const int DIST_wall_RL = 90;      //与左右墙壁的距离 1
const int DIST_wall_RL = 87;      //与左右墙壁的距离

/*---定义全局变量---*/
/*---容器类---*/
std::vector<std::vector<int>> mark(1,std::vector<int>(1,0));                                   //定义mark容器（二维容器），容器初始为1行1列    1=为已访问
std::vector<std::vector<std::vector<int>>> wall(2,std::vector<std::vector<int>>(2,std::vector<int>(2,0)));    //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息   1=墙壁；0=通路

/*---机器人坐标---*/
int rob_x0 = 0;     //机器人起点X坐标
int rob_y0 = 0;     //机器人起点Y坐标
int rob_x = 0;      //机械人当前X坐标（初始为0）
int rob_y = 0;      //机器人当前Y坐标（初始为0）
int rob_dir = 0;    //机器人当前的朝向（初始为0）

/*---递归算法---*/


/*---PID算法---*/
/*--成功过的方案--*/
//~ const float Kp = 2.8, Ki = 0.006, Kd = 0.08;   //定义PD的参数16-2此方案成功过，就是有点摇摇晃晃 墙壁为1
//~ const float Kp = 0.2, Ki =0, Kd = 1;   //定义PD的参数16-2
/*--在尝试的方案--*/

const float Kp = 0.22, Ki =0.004, Kd = 1;   //定义PD的参数16-2


//~ float Kpid;                             //定义PID计算后输出的倍率
int Kpid_speed;
const float Kpid_base = 1;				//定义基本角速度和速度
const int speed_l = 16;					//定义基本左轮速度
const int speed_r = speed_l - 1;		//定义基本右轮速度
//~ int speed_r = speed_l - 1;		//定义基本右轮速度
const int speed_xianfu = 2;				//定义kpid限幅
int err, err_old, err_sum, err_diff;	//定义误差，上一次误差，积分误差，微分误差
const int eI_max = 200;                 //定义积分限幅

/*---目标点的坐标---*/
const int target_x = 3;   //声明目标点的X坐标
const int target_y = 4;   //声明目标点的Y坐标

/*---定义结束---*/