mirror of
https://gitee.com/yyz_o/bk_bishe_pi.git
synced 2025-09-07 23:21:26 +00:00
44 lines
1.7 KiB
C++
44 lines
1.7 KiB
C++
#include<iostream>
|
||
#include<string>
|
||
#include<vector>
|
||
#include<cstddef>
|
||
#include"eyebot++.h"
|
||
#include"maze_parameter.h"
|
||
#include"maze_func.h"
|
||
using namespace std;
|
||
|
||
/*---定义数据---*/
|
||
/*---定义全局常量---*/
|
||
const int DIST_cell = 360 ; //单元格长度
|
||
const int DIST_wall = 130 ; //与墙壁的距离
|
||
|
||
/*---定义全局变量---*/
|
||
/*---容器类---*/
|
||
vector<vector<int>> mark(1,vector<int>(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问
|
||
vector<vector<vector<int>>> wall(2,vector<vector<int>>(2,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 = 1, Ki = 0, Kd = 0.5; //定义PID的参数
|
||
float Kpid; //定义PID计算后输出的倍率
|
||
float Kpid_speed;
|
||
const int Kpid_base = 2, speed = 24; //定义基本角速度和速度
|
||
const int speed2 = 0.75*speed; //快到目的地时减速
|
||
int speed1;
|
||
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||
const int eI_max = 20; //定义积分限幅
|
||
|
||
/*---目标点的坐标---*/
|
||
const int target_x = 4; //声明目标点的X坐标
|
||
const int target_y = 4; //声明目标点的Y坐标
|
||
|
||
/*---定义结束---*/ |