mirror of
https://gitee.com/yyz_o/bk_bishe_pi.git
synced 2025-09-07 23:21:26 +00:00
Compare commits
22 Commits
da75133de4
...
master
| Author | SHA1 | Date | |
|---|---|---|---|
|
c212d4add1 | ||
|
|
ed74171022 | ||
|
|
e5fac173be | ||
|
|
0b8d29917f | ||
|
|
9ba913646c | ||
|
|
efa0d49694 | ||
|
|
a30de465d1 | ||
|
|
14b3d14170 | ||
|
|
9d96af2ff3 | ||
|
|
00e0d9366d | ||
|
|
1155ae6f6d | ||
|
|
df42d6ce7f | ||
|
|
47a4473330 | ||
|
|
5ce7796b14 | ||
|
|
137218ebe5 | ||
|
|
b933cdfc1b | ||
|
|
a81594bf68 | ||
|
|
75f7336601 | ||
|
|
69f91d3b52 | ||
|
|
95c2e525c4 | ||
|
|
9cd5540936 | ||
|
|
6879dec68c |
20
bot_code/makefile
Normal file
20
bot_code/makefile
Normal file
@@ -0,0 +1,20 @@
|
||||
|
||||
cxx := gccarm
|
||||
|
||||
file_cpp := $(wildcard *.cpp)
|
||||
file_o := /home/pi/usr/maze.x
|
||||
|
||||
$(file_o) : $(file_cpp)
|
||||
@$(cxx) $^ -o $@
|
||||
|
||||
maze : $(file_o)
|
||||
|
||||
clean :
|
||||
@rm -rf $(file_o)
|
||||
|
||||
debug :
|
||||
@echo $(cxx)
|
||||
@echo $(file_cpp)
|
||||
@echo $(file_o)
|
||||
|
||||
.PHONY : maze clean debug
|
||||
47
bot_code/maze_func.h
Normal file
47
bot_code/maze_func.h
Normal file
@@ -0,0 +1,47 @@
|
||||
#pragma once
|
||||
|
||||
/*---PART1---*/
|
||||
/*---以下为递归算法探索部分涉及到的函数声明---*/
|
||||
bool check_mark(); //检查单元格各方面是否已被确认
|
||||
void maze_entry(int x, int y, int dir, int open); //maze_entry函数,用于写入到wall数组中，1：有墙 0：通路
|
||||
bool unmarked(int y, int x, int dir); //检查该方向是否已被标记
|
||||
void explore(); //递归函数
|
||||
/*---以上为递归算法探索部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART2---*/
|
||||
/*---以下为pid控制和行驶部分涉及到的函数声明---*/
|
||||
void xneighbor(int x, int dir); //X坐标更新
|
||||
void yneighbor(int y, int dir); //Y坐标更新
|
||||
void eI_xianfu(); //对PID控制的积分部分I限幅
|
||||
void pid_speed_xianfu(); //对PID控制的输出部分限幅
|
||||
void PID_AL(); //位置式PID算法
|
||||
void PIDStraight(); //PID控制下的直线行驶
|
||||
void go_to(int dir); //行驶到指定单元格
|
||||
void BOTturn(int turn); //自定义转向函数
|
||||
/*---以上为pid控制和行驶部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART3---*/
|
||||
/*---以下为洪水填充算法部分涉及到的函数声明---*/
|
||||
void flood(int *map, int *copy_wall); //洪水填充算法
|
||||
/*---以上为洪水填充算法部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART4---*/
|
||||
/*---以下为路径算法部分涉及到的函数声明---*/
|
||||
void build_path(int i, int j, int len, int *path, int *map, int *copy_wall); //路径算法
|
||||
void drive_path(int len, int *path); //移动到目的地
|
||||
/*---以上为路径算法部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART5---*/
|
||||
/*---以下为数组构建部分涉及到的函数声明---*/
|
||||
void array_negative_one(int size, int *arr); //将数组初始化为-1
|
||||
void array_copy_mark(int size_x, int size_y, int *copy_mark); //复制一份mark数组，便于后续频繁读取提升性能
|
||||
void array_copy_wall(int size_x, int size_y, int *copy_wall); //复制一份wall数组，便于后续频繁读取提升性能
|
||||
/*---以上为数组构建部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART6---*/
|
||||
/*---以下为数组打印部分涉及到的函数声明---*/
|
||||
void output_arr2D(int size_x, int size_y, int *arr); //打印所有二维数组数组
|
||||
void output_arrwall(int size_x, int size_y, int *arr); //打印wall数组
|
||||
void output_arrpath(int size, int *arr); //打印path数组
|
||||
/*---以上为数组打印部分涉及到的函数声明---*/
|
||||
|
||||
42
bot_code/maze_func_array.cpp
Normal file
42
bot_code/maze_func_array.cpp
Normal file
@@ -0,0 +1,42 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
/*---从vector容器复制mark信息到数组函数---*/
|
||||
void array_copy_mark(int size_x, int size_y, int *copy_mark)
|
||||
{
|
||||
for (int i = 0; i < size_x; i++)
|
||||
{
|
||||
for (int j = 0; j < size_y; j++)
|
||||
{
|
||||
copy_mark[i * size_y + j] = mark[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*---从vector容器复制wall信息到数组函数---*/
|
||||
void array_copy_wall(int size_x, int size_y, int *copy_wall)
|
||||
{
|
||||
for (int i = 0; i <= size_x; i++)
|
||||
{
|
||||
for (int j = 0; j <= size_y; j++)
|
||||
{
|
||||
for(int k = 0; k < 2; k++)
|
||||
{
|
||||
copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k]; //注释：k = 0记录的是单元格下方的数据，1记录的是单元格左边的数据，所以转为一维数组后，i*...+j*...+k,k=1为左侧数据，k=0为下方墙壁。
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void array_negative_one(int size, int *arr) //将数组初始化为-1
|
||||
{
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
arr[i] = -1;
|
||||
}
|
||||
}
|
||||
261
bot_code/maze_func_explore.cpp
Normal file
261
bot_code/maze_func_explore.cpp
Normal file
@@ -0,0 +1,261 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include<cstddef>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---检查单元格各方向是否已被到访---*/
|
||||
bool check_mark()
|
||||
{ bool check = false;
|
||||
int x, y;
|
||||
x = rob_x; //机器人当前x坐标
|
||||
y = rob_y; //机器人当前y坐标
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
|
||||
bool N_mark;
|
||||
bool W_mark;
|
||||
bool S_mark;
|
||||
bool E_mark;
|
||||
|
||||
if (x > 0)
|
||||
{
|
||||
W_mark = mark[x-1][y];
|
||||
}
|
||||
else
|
||||
{
|
||||
W_mark = true;
|
||||
}
|
||||
|
||||
if (y > 0)
|
||||
{
|
||||
S_mark = mark[x][y-1];
|
||||
}
|
||||
else
|
||||
{
|
||||
S_mark = true;
|
||||
}
|
||||
|
||||
if (x < size_x - 1)
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
|
||||
if (y < size_y - 1)
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
|
||||
bool N_road = (!wall[x][y+1][0] && N_mark);
|
||||
bool W_road = (!wall[x][y][1] && W_mark);
|
||||
bool S_road = (!wall[x][y][0] && S_mark);
|
||||
bool E_road = (!wall[x+1][y][1] && E_mark);
|
||||
|
||||
if (N_road && W_road && S_road && E_road)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
return check;
|
||||
}
|
||||
|
||||
/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/
|
||||
void maze_entry(int x, int y, int dir, int open)
|
||||
{
|
||||
int maze_dir = (dir + 4) % 4;
|
||||
|
||||
switch (maze_dir)
|
||||
{
|
||||
case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
|
||||
if (y == static_cast<int>(mark[0].size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
for (size_t i = 0; i < mark.size(); i++)
|
||||
{
|
||||
mark[i].emplace_back(0);
|
||||
}
|
||||
for (size_t i = 0; i < wall.size(); i++)
|
||||
{
|
||||
wall[i].emplace_back(std::vector<int>(2,0));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x][y+1][0] = !open;
|
||||
break;
|
||||
|
||||
case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
|
||||
if (x == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
//~ std::vector<int> mark_insert(mark[0].size(), 0);
|
||||
//~ mark.insert(mark.begin(), mark_insert);
|
||||
|
||||
//~ std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
|
||||
//~ wall.insert(wall.begin(), wall_insert);
|
||||
|
||||
mark.emplace(mark.begin(), std::vector<int>(mark[0].size(), 0)); //在mark头部插入一个内层
|
||||
wall.emplace(wall.begin(), std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0))); //在wall头部插入一个内层
|
||||
rob_x++;
|
||||
rob_x0++;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x][y][1] = !open;
|
||||
break;
|
||||
|
||||
case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
|
||||
if (y == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
for (size_t i = 0; i < mark.size(); i++)
|
||||
{
|
||||
mark[i].emplace(mark[i].begin(), 0);
|
||||
}
|
||||
for (size_t i = 0; i < wall.size(); i++)
|
||||
{
|
||||
wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
|
||||
}
|
||||
rob_y++;
|
||||
rob_y0++;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x][y][0] = !open;
|
||||
break;
|
||||
|
||||
case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息
|
||||
if (x == static_cast<int>(mark.size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
mark.emplace_back(std::vector<int>(mark[0].size(), 0));
|
||||
wall.emplace_back(std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0)));
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x+1][y][1] = !open;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*---检查该方向是否已被标记---*/
|
||||
bool unmarked(int x, int y, int dir)
|
||||
{
|
||||
bool check = false;
|
||||
int maze_dir = (dir + 4) % 4;
|
||||
switch(maze_dir)
|
||||
{
|
||||
case 0:
|
||||
if (y < (static_cast<int>(mark[0].size()) - 1))
|
||||
{
|
||||
if (mark[x][y+1] == 0)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (x > 0)
|
||||
{
|
||||
if (mark[x-1][y] == 0)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if (y > 0)
|
||||
{
|
||||
if (mark[x][y-1] == 0)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
if (x < (static_cast<int>(mark.size()) - 1))
|
||||
{
|
||||
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; //将当前的单元格设置为已到访
|
||||
F_open = PSDGetRaw(1) > DIST_cell; //获取前侧空间状况
|
||||
L_open = PSDGetRaw(2) > DIST_cell; //获取左侧空间状况
|
||||
R_open = PSDGetRaw(3) > 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();
|
||||
} //递归结束
|
||||
63
bot_code/maze_func_flood.cpp
Normal file
63
bot_code/maze_func_flood.cpp
Normal file
@@ -0,0 +1,63 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
void flood(int *map, int *copy_wall)
|
||||
{
|
||||
int num = 0;
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
map[0] = 0;
|
||||
|
||||
do
|
||||
{
|
||||
for (int i = 0; i < size_x; i++)
|
||||
{
|
||||
for (int j = 0; j < size_y; j++)
|
||||
{
|
||||
if (map[(i * size_y) + j] != -1)
|
||||
{
|
||||
if (i > 0)
|
||||
{
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子
|
||||
{
|
||||
map[((i-1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (i < size_x - 1)
|
||||
{
|
||||
if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子
|
||||
{
|
||||
map[((i+1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (j > 0)
|
||||
{
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1)
|
||||
{
|
||||
map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (j < size_y - 1)
|
||||
{
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1)
|
||||
{
|
||||
map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1 && num < (size_x * size_y));
|
||||
}
|
||||
90
bot_code/maze_func_goto.cpp
Normal file
90
bot_code/maze_func_goto.cpp
Normal file
@@ -0,0 +1,90 @@
|
||||
#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(1000);
|
||||
VWWait(); //等待下一条指令
|
||||
|
||||
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向
|
||||
xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
|
||||
yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
|
||||
}
|
||||
58
bot_code/maze_func_output.cpp
Normal file
58
bot_code/maze_func_output.cpp
Normal file
@@ -0,0 +1,58 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---打印数组检查错误---*/
|
||||
/*---打印数组mark,map,copy_map等二维数组所用的函数---*/
|
||||
void output_arr2D(int size_x, int size_y, int *arr)
|
||||
{
|
||||
for (int i = size_y - 1; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j < size_x; j++)
|
||||
{
|
||||
cout << arr[(j * size_y) + i] << " ";
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/*---打印数组wall---*/
|
||||
void output_arrwall(int size_x, int size_y, int *arr)
|
||||
{
|
||||
for (int i = size_y; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j <= size_x; j++)
|
||||
{
|
||||
if (arr[(j * (size_y + 1) * 2) + (i * 2) + 1] == 1)
|
||||
{
|
||||
cout << "|" ;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << " " ;
|
||||
}
|
||||
|
||||
if (arr[(j * (size_y + 1) * 2) + (i * 2) + 0] == 1)
|
||||
{
|
||||
cout << "_" ;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << " " ;
|
||||
}
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void output_arrpath(int size, int *arr)
|
||||
{
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
cout << arr[i] << " ";
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
51
bot_code/maze_func_path.cpp
Normal file
51
bot_code/maze_func_path.cpp
Normal file
@@ -0,0 +1,51 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
/*---解迷宫路径到path数组函数---*/
|
||||
void build_path(int i, int j, int len, int *path, int *map, int *copy_wall)
|
||||
{
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
|
||||
for (int k = len -1; k >= 0; k--)
|
||||
{
|
||||
if (i > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == k) //前一个单位的东面是当前格，即当前格子的左侧为上一个格子
|
||||
{
|
||||
i--;
|
||||
path[k] = 3;
|
||||
}
|
||||
else if (i < size_x - 1 && !copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
|
||||
{
|
||||
i++;
|
||||
path[k] = 1;
|
||||
}
|
||||
else if(j > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i * size_y) + (j-1)] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
|
||||
{
|
||||
j--;
|
||||
path[k] = 0;
|
||||
}
|
||||
else if (j < size_y - 1 && !copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
|
||||
{
|
||||
j++;
|
||||
path[k] = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
LCDPrintf("ERROR");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*---通过path数组移动到目标点函数---*/
|
||||
void drive_path(int len, int *path)
|
||||
{
|
||||
for(int i = 0; i < len; i++)
|
||||
{
|
||||
go_to(path[i]);
|
||||
}
|
||||
}
|
||||
137
bot_code/maze_func_pid.cpp
Normal file
137
bot_code/maze_func_pid.cpp
Normal file
@@ -0,0 +1,137 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---对PID控制的积分部分I限幅---*/
|
||||
void eI_xianfu()
|
||||
{
|
||||
if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
|
||||
{
|
||||
err_sum = eI_max;
|
||||
}
|
||||
else if (err_sum <= -eI_max)
|
||||
{
|
||||
err_sum = -eI_max;
|
||||
}
|
||||
else
|
||||
{
|
||||
//此处为空;
|
||||
}
|
||||
}
|
||||
|
||||
void pid_speed_xianfu()
|
||||
{
|
||||
if (Kpid_speed >= speed_xianfu)
|
||||
{
|
||||
Kpid_speed = speed_xianfu;
|
||||
}
|
||||
else if (Kpid_speed <= -speed_xianfu)
|
||||
{
|
||||
Kpid_speed = -speed_xianfu;
|
||||
}
|
||||
else
|
||||
{
|
||||
//此处为空
|
||||
}
|
||||
}
|
||||
|
||||
/*---位置式PID算法---*/
|
||||
void PID_AL()
|
||||
{
|
||||
err_sum += err; //积分累计误差值
|
||||
eI_xianfu(); //积分部分限幅
|
||||
err_diff = err - err_old; //微分部分误差值
|
||||
/*---打印输出PID控制的各项参数1---*/
|
||||
//~ cout << "err:" << err << " " ;
|
||||
//~ cout << "old_err:" << err_old << " " ;
|
||||
//~ cout << "err_sum:" << err_sum << " " ;
|
||||
//~ cout << "err_diff:" << err_diff << " " ;
|
||||
/*---打印结束---*/
|
||||
err_old = err; //将误差幅值到上一次误差
|
||||
|
||||
//~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
|
||||
Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base;
|
||||
//~ Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
|
||||
|
||||
pid_speed_xianfu();
|
||||
//~ speed_r = speed_l + Kpid_speed;
|
||||
|
||||
|
||||
/*---打印输出PID控制的各项参数2---*/
|
||||
//~ cout << "PID_out:" << Kpid_speed << endl ;
|
||||
//~ cout << "PID_out:" << Kpid_speed << " " ;
|
||||
/*---打印结束---*/
|
||||
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
|
||||
|
||||
MOTORDriveRaw(2,speed_l - 2 + Kpid_speed);
|
||||
//~ MOTORDriveRaw(2,speed_r);
|
||||
MOTORDriveRaw(1,speed_l);
|
||||
}
|
||||
|
||||
/*---PID控制下的直线行驶---*/
|
||||
void PIDStraight()
|
||||
{
|
||||
int bot_move; //定义基础角速度和速度,和移动距离
|
||||
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
|
||||
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
|
||||
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
|
||||
//~ cout << x_1 << " " << y_1 << " " << phi_1 << endl;
|
||||
|
||||
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
|
||||
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
|
||||
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
|
||||
|
||||
//~ MOTORDriveRaw(2,speed_r);
|
||||
MOTORDriveRaw(2,speed_l - 2);
|
||||
MOTORDriveRaw(1,speed_l);
|
||||
|
||||
|
||||
do
|
||||
{
|
||||
//~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
|
||||
//~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
|
||||
//~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
|
||||
|
||||
F_PSD = (PSDGetRaw(1) - 2) * 0.3 + F_PSD * 0.7; //读取前方和墙壁的距离
|
||||
L_PSD = (PSDGetRaw(2) - 16) * 0.3 + L_PSD * 0.7; //读取左侧和墙壁的距离
|
||||
R_PSD = (PSDGetRaw(3)) * 0.3 + R_PSD * 0.7; //读取右侧和墙壁的距离
|
||||
|
||||
/*---打印输出PSD数值---*/
|
||||
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
|
||||
/*---打印结束---*/
|
||||
|
||||
if (20<L_PSD && L_PSD<140 && 20<R_PSD && R_PSD<140) //如果两侧都有墙的情况
|
||||
{
|
||||
err = L_PSD - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else if (20<L_PSD && L_PSD<140) //只有左侧有墙的情况
|
||||
{
|
||||
err = L_PSD - DIST_wall_RL;
|
||||
PID_AL();
|
||||
}
|
||||
else if (20<R_PSD && R_PSD<140) //只有右侧有墙的情况
|
||||
{
|
||||
err = DIST_wall_RL - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else
|
||||
{
|
||||
L_PSD = L_PSD % 300;
|
||||
R_PSD = R_PSD % 300;
|
||||
err = L_PSD - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
VWGetPosition(&x_2, &y_2, &phi_2);
|
||||
bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
|
||||
cout << "move:" << bot_move << endl;
|
||||
|
||||
}while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||||
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
57
bot_code/maze_func_turn.cpp
Normal file
57
bot_code/maze_func_turn.cpp
Normal file
@@ -0,0 +1,57 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
void BOTturn(int turn)
|
||||
{
|
||||
if (turn == -1) //右转
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,20);
|
||||
MOTORDriveRaw(2,-18);
|
||||
|
||||
} while (abs(ENCODERRead(2)) < 240);
|
||||
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
else if (turn == 1) //左转
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,-20);
|
||||
MOTORDriveRaw(2,18);
|
||||
|
||||
} while (abs(ENCODERRead(2)) < 240);
|
||||
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
else if (turn == 2) //掉头
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,20);
|
||||
MOTORDriveRaw(2,-18);
|
||||
|
||||
} while (abs(ENCODERRead(2)) < 550);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
//~ speed_r = speed_l - Kpid_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "errer" << endl;
|
||||
}
|
||||
}
|
||||
42
bot_code/maze_main.cpp
Normal file
42
bot_code/maze_main.cpp
Normal file
@@ -0,0 +1,42 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---下面是主函数---*/
|
||||
int main()
|
||||
{
|
||||
explore(); //使用递归算法构建地图
|
||||
wall[0][0][0] = 1;
|
||||
|
||||
const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度
|
||||
const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度
|
||||
|
||||
int copy_mark[mazesize_x * mazesize_y] = {}; //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0
|
||||
int copy_wall[(mazesize_x + 1) * (mazesize_y + 1) * 2] = {}; //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0
|
||||
|
||||
array_copy_mark(mazesize_x, mazesize_y, copy_mark); //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能
|
||||
array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
|
||||
|
||||
output_arr2D(mazesize_x, mazesize_y, copy_mark); //打印地图mark信息
|
||||
output_arrwall(mazesize_x, mazesize_y, copy_wall); //打印墙壁wall信息
|
||||
|
||||
int map[mazesize_x * mazesize_y] = {}; //创建最短路径求解地图并初始化为0
|
||||
array_negative_one(mazesize_x * mazesize_y, map); //将map数组初始化为-1
|
||||
|
||||
flood(map, copy_wall); //洪水填充算法
|
||||
output_arr2D(mazesize_x, mazesize_y, map); //打印map的数组信息
|
||||
|
||||
const int len = map[((target_x - 1) * mazesize_y) + (target_y -1)]; //迷宫的终点（人为确定）
|
||||
int path[len] = {}; //创建最短路径结果地图并初始化为0
|
||||
|
||||
build_path(target_x - 1, target_y - 1, len, path, map, copy_wall); //构建出最短路径path数组
|
||||
drive_path(len, path); //移动到指定目标点
|
||||
|
||||
output_arrpath(len, path); //打印path数组信息，及小车每一步的行驶方向
|
||||
|
||||
}
|
||||
50
bot_code/maze_parameter.cpp
Normal file
50
bot_code/maze_parameter.cpp
Normal file
@@ -0,0 +1,50 @@
|
||||
#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 = 90; //与墙壁的距离
|
||||
const int DIST_wall_RL = 90; //与左右墙壁的距离
|
||||
|
||||
/*---定义全局变量---*/
|
||||
/*---容器类---*/
|
||||
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 = 0.4, Ki = 0, Kd = 0.25; //定义PID的参数17
|
||||
//~ const float Kp = 0.825, Ki = 0.0081, Kd = 0.175; //定义PID的参数17
|
||||
//~ const float Kp = 0.80, Ki = 0.0076, Kd = 0.2; //定义PID的参数18
|
||||
const float Kp = 1.6, Ki = 0.008, Kd = 0.18; //定义PID的参数18
|
||||
//~ float Kpid; //定义PID计算后输出的倍率
|
||||
int Kpid_speed;
|
||||
const float Kpid_base = 1; //定义基本角速度和速度
|
||||
const int speed_l = 16; //定义基本左轮速度
|
||||
//~ const int speed_r = speed_l - 2; //定义基本右轮速度
|
||||
int speed_r = speed_l - 2; //定义基本右轮速度
|
||||
const int speed_xianfu = 2; //定义kpid限幅
|
||||
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||||
const int eI_max = 200; //定义积分限幅
|
||||
|
||||
/*---目标点的坐标---*/
|
||||
const int target_x = 4; //声明目标点的X坐标
|
||||
const int target_y = 4; //声明目标点的Y坐标
|
||||
|
||||
/*---定义结束---*/
|
||||
44
bot_code/maze_parameter.h
Normal file
44
bot_code/maze_parameter.h
Normal file
@@ -0,0 +1,44 @@
|
||||
#pragma once
|
||||
#include<vector>
|
||||
|
||||
/*---声明数据---*/
|
||||
/*---声明全局常量---*/
|
||||
extern const int DIST_cell; //单元格长度
|
||||
extern const int DIST_move; //移动的单元格距离
|
||||
extern const int DIST_wall_F; //与前面墙壁的距离
|
||||
extern const int DIST_wall_RL; //与左右墙壁的距离
|
||||
|
||||
/*---声明全局变量---*/
|
||||
|
||||
/*---声明全局容器---*/
|
||||
extern std::vector<std::vector<int>> mark; //声明mark容器（二维容器）
|
||||
extern std::vector<std::vector<std::vector<int>>> wall; //声明wall容器（三维容器）
|
||||
|
||||
/*---声明机器人坐标---*/
|
||||
extern int rob_x0; //机器人起点X坐标
|
||||
extern int rob_y0; //机器人起点Y坐标
|
||||
extern int rob_x; //机械人当前X坐标
|
||||
extern int rob_y; //机器人当前Y坐标
|
||||
extern int rob_dir; //机器人当前的朝向
|
||||
|
||||
/*---各种全局量声明结束---*/
|
||||
|
||||
/*---递归算法---*/
|
||||
|
||||
/*---PID算法---*/
|
||||
extern const float Kp, Ki, Kd; //声明PID的参数
|
||||
//~ extern float Kpid; //定义PID计算后输出的倍率
|
||||
extern int Kpid_speed; //定义计算后的速度
|
||||
extern const float Kpid_base; //定义速度基础倍数速度
|
||||
extern const int speed_l; //定义左轮基本速度
|
||||
//~ extern const int speed_r; //定义右轮基本速度
|
||||
extern int speed_r; //定义右轮基本速度
|
||||
extern const int speed_xianfu; //kpid输出速度的限幅
|
||||
extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||||
extern const int eI_max; //定义积分限幅
|
||||
|
||||
/*---目标点坐标---*/
|
||||
extern const int target_x; //声明目标点的X坐标
|
||||
extern const int target_y; //声明目标点的Y坐标
|
||||
|
||||
/*---声明结束---*/
|
||||
16
bot_code/全局变量.txt
Normal file
16
bot_code/全局变量.txt
Normal file
@@ -0,0 +1,16 @@
|
||||
几个数组：
|
||||
wall，mark，maze....
|
||||
|
||||
机器人坐标：
|
||||
rob_x 机器人X坐标
|
||||
rob_y 机器人Y坐标
|
||||
rob_dir 机器人朝向
|
||||
|
||||
迷宫基础数据：
|
||||
DIST_cell 单个单元格的长度
|
||||
DIST_wall PID直线的时候，检测与墙壁的距离值
|
||||
|
||||
PID：
|
||||
err，err_old，err_sum，err_diff, eI_max 误差，上一次误差，积分累计误差，微分误差变化，积分限幅
|
||||
Kp, Ki, Kd, Kpid PID控制的参数P,I,D以及算法输出的倍率参数
|
||||
|
||||
5
bot_code/名称解释.txt
Normal file
5
bot_code/名称解释.txt
Normal file
@@ -0,0 +1,5 @@
|
||||
头文件:mazetool.h
|
||||
函数文件:mazetool.cpp
|
||||
全局:
|
||||
rob_y:机器人在迷宫地图中的Y坐标值
|
||||
rob_x:机器人在迷宫地图中的x坐标值
|
||||
22
bot_code_turn/makefile
Normal file
22
bot_code_turn/makefile
Normal file
@@ -0,0 +1,22 @@
|
||||
|
||||
cxx := gccarm
|
||||
#~ cxx := g++sim
|
||||
|
||||
file_cpp := $(wildcard *.cpp)
|
||||
file_o := /home/pi/usr/maze2.x
|
||||
#~ file_o := ../maze2.x
|
||||
|
||||
$(file_o) : $(file_cpp)
|
||||
@$(cxx) $^ -o $@
|
||||
|
||||
maze : $(file_o)
|
||||
|
||||
clean :
|
||||
@rm -rf $(file_o)
|
||||
|
||||
debug :
|
||||
@echo $(cxx)
|
||||
@echo $(file_cpp)
|
||||
@echo $(file_o)
|
||||
|
||||
.PHONY : maze clean debug
|
||||
50
bot_code_turn/maze_func.h
Normal file
50
bot_code_turn/maze_func.h
Normal file
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
/*---PART1---*/
|
||||
/*---以下为递归算法探索部分涉及到的函数声明---*/
|
||||
bool check_mark(); //检查单元格各方面是否已被确认
|
||||
void maze_entry(int x, int y, int dir, int open); //maze_entry函数,用于写入到wall数组中，1：有墙 0：通路
|
||||
bool unmarked(int y, int x, int dir); //检查该方向是否已被标记
|
||||
void explore(); //递归函数
|
||||
/*---以上为递归算法探索部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART2---*/
|
||||
/*---以下为pid控制和行驶部分涉及到的函数声明---*/
|
||||
void xneighbor(int x, int dir); //X坐标更新
|
||||
void yneighbor(int y, int dir); //Y坐标更新
|
||||
void eI_xianfu(); //对PID控制的积分部分I限幅
|
||||
void pid_speed_xianfu(); //对PID控制的输出部分限幅
|
||||
void PID_AL(); //位置式PID算法
|
||||
void PIDStraight(); //PID控制下的直线行驶
|
||||
void go_to(int dir); //行驶到指定单元格
|
||||
void BOTturn(int turn); //自定义转向函数
|
||||
/*---以上为pid控制和行驶部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART3---*/
|
||||
/*---以下为洪水填充算法部分涉及到的函数声明---*/
|
||||
void flood(int *map, int *copy_wall); //洪水填充算法
|
||||
/*---以上为洪水填充算法部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART4---*/
|
||||
/*---以下为路径算法部分涉及到的函数声明---*/
|
||||
void build_path(int i, int j, int len, int *path, int *map, int *copy_wall); //路径算法
|
||||
void drive_path(int len, int *path); //移动到目的地
|
||||
/*---以上为路径算法部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART5---*/
|
||||
/*---以下为数组构建部分涉及到的函数声明---*/
|
||||
void array_negative_one(int size, int *arr); //将数组初始化为-1
|
||||
void array_copy_mark(int size_x, int size_y, int *copy_mark); //复制一份mark数组，便于后续频繁读取提升性能
|
||||
void array_copy_wall(int size_x, int size_y, int *copy_wall); //复制一份wall数组，便于后续频繁读取提升性能
|
||||
/*---以上为数组构建部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART6---*/
|
||||
/*---以下为数组打印部分涉及到的函数声明---*/
|
||||
void output_arr2D(int size_x, int size_y, int *arr); //打印所有二维数组数组
|
||||
void output_arrwall(int size_x, int size_y, int *arr); //打印wall数组
|
||||
void output_arrpath(int size, int *arr); //打印path数组
|
||||
/*---以上为数组打印部分涉及到的函数声明---*/
|
||||
|
||||
/*---以下为目标点输入函数声明---*/
|
||||
void cin_targey(); //输入目标点
|
||||
/*---以上为目标点输入函数声明---*/
|
||||
41
bot_code_turn/maze_func_array.cpp
Normal file
41
bot_code_turn/maze_func_array.cpp
Normal file
@@ -0,0 +1,41 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
/*---从vector容器复制mark信息到数组函数---*/
|
||||
void array_copy_mark(int size_x, int size_y, int *copy_mark)
|
||||
{
|
||||
for (int i = 0; i < size_x; i++)
|
||||
{
|
||||
for (int j = 0; j < size_y; j++)
|
||||
{
|
||||
copy_mark[i * size_y + j] = mark[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*---从vector容器复制wall信息到数组函数---*/
|
||||
void array_copy_wall(int size_x, int size_y, int *copy_wall)
|
||||
{
|
||||
for (int i = 0; i <= size_x; i++)
|
||||
{
|
||||
for (int j = 0; j <= size_y; j++)
|
||||
{
|
||||
for(int k = 0; k < 2; k++)
|
||||
{
|
||||
copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k]; //注释：k = 0记录的是单元格下方的数据，1记录的是单元格左边的数据，所以转为一维数组后，i*...+j*...+k,k=1为左侧数据，k=0为下方墙壁。
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void array_negative_one(int size, int *arr) //将数组初始化为-1
|
||||
{
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
arr[i] = -1;
|
||||
}
|
||||
}
|
||||
26
bot_code_turn/maze_func_cin.cpp
Normal file
26
bot_code_turn/maze_func_cin.cpp
Normal file
@@ -0,0 +1,26 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
void cin_targey()
|
||||
{
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
|
||||
int num = 0;
|
||||
do
|
||||
{
|
||||
if (!num)
|
||||
{
|
||||
cout << "您输入的目标点超出迷宫范围，请重新输入" << endl;
|
||||
}
|
||||
cout << "请输入目标点的X,Y坐标值 用空格隔开并回车" << endl;
|
||||
cin >> target_x >> target_y;
|
||||
cout << endl;
|
||||
num++;
|
||||
} while (target_x <= size_x && target_y <= size_y);
|
||||
cout << endl;
|
||||
}
|
||||
289
bot_code_turn/maze_func_explore.cpp
Normal file
289
bot_code_turn/maze_func_explore.cpp
Normal file
@@ -0,0 +1,289 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include<cstddef>
|
||||
#include<cstdlib>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---检查单元格各方向是否已被到访---*/
|
||||
bool check_mark()
|
||||
{ bool check = false;
|
||||
int x, y;
|
||||
x = rob_x; //机器人当前x坐标
|
||||
y = rob_y; //机器人当前y坐标
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
|
||||
bool N_mark;
|
||||
bool W_mark;
|
||||
bool S_mark;
|
||||
bool E_mark;
|
||||
|
||||
if (y < size_y - 1)
|
||||
{
|
||||
if (!wall[x][y+1][0])
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
|
||||
if (x > 0)
|
||||
{
|
||||
if (!wall[x][y][1])
|
||||
{
|
||||
W_mark = mark[x-1][y];
|
||||
}
|
||||
else
|
||||
{
|
||||
W_mark = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
W_mark = true;
|
||||
}
|
||||
|
||||
if (y > 0)
|
||||
{
|
||||
if (!wall[x][y][0])
|
||||
{
|
||||
S_mark = mark[x][y-1];
|
||||
}
|
||||
else
|
||||
{
|
||||
S_mark = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
S_mark = true;
|
||||
}
|
||||
|
||||
if (x < size_x - 1)
|
||||
{
|
||||
if (!wall[x+1][y][1])
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
|
||||
if (N_mark && W_mark && S_mark && E_mark)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
return check;
|
||||
}
|
||||
|
||||
/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/
|
||||
void maze_entry(int x, int y, int dir, int open)
|
||||
{
|
||||
int maze_dir = (dir + 4) % 4;
|
||||
|
||||
switch (maze_dir)
|
||||
{
|
||||
case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
|
||||
if (y == static_cast<int>(mark[0].size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
for (size_t i = 0; i < mark.size(); i++)
|
||||
{
|
||||
mark[i].emplace_back(0);
|
||||
}
|
||||
for (size_t i = 0; i < wall.size(); i++)
|
||||
{
|
||||
wall[i].emplace_back(std::vector<int>(2,0));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
wall[x][y+1][0] = !open;
|
||||
break;
|
||||
|
||||
case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
|
||||
if (x == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
// std::vector<int> mark_insert(mark[0].size(), 0);
|
||||
// mark.insert(mark.begin(), mark_insert);
|
||||
|
||||
// std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
|
||||
// wall.insert(wall.begin(), wall_insert);
|
||||
|
||||
mark.emplace(mark.begin(), std::vector<int>(mark[0].size(), 0)); //在mark头部插入一个内层
|
||||
wall.emplace(wall.begin(), std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0))); //在wall头部插入一个内层
|
||||
rob_x++;
|
||||
rob_x0++;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
wall[x][y][1] = !open;
|
||||
break;
|
||||
|
||||
case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
|
||||
if (y == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
for (size_t i = 0; i < mark.size(); i++)
|
||||
{
|
||||
mark[i].emplace(mark[i].begin(), 0);
|
||||
}
|
||||
for (size_t i = 0; i < wall.size(); i++)
|
||||
{
|
||||
wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
|
||||
}
|
||||
rob_y++;
|
||||
rob_y0++;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
wall[x][y][0] = !open;
|
||||
break;
|
||||
|
||||
case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息
|
||||
if (x == static_cast<int>(mark.size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
|
||||
{
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
mark.emplace_back(std::vector<int>(mark[0].size(), 0));
|
||||
wall.emplace_back(std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0)));
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
wall[x+1][y][1] = !open;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*---检查该方向是否已被标记---*/
|
||||
bool unmarked(int x, int y, int dir)
|
||||
{
|
||||
bool check = false;
|
||||
int maze_dir = (dir + 4) % 4;
|
||||
switch(maze_dir)
|
||||
{
|
||||
case 0:
|
||||
if (y < (static_cast<int>(mark[0].size()) - 1))
|
||||
{
|
||||
if (mark[x][y+1] == 0)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (x > 0)
|
||||
{
|
||||
if (mark[x-1][y] == 0)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if (y > 0)
|
||||
{
|
||||
if (mark[x][y-1] == 0)
|
||||
{
|
||||
check = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
if (x < (static_cast<int>(mark.size()) - 1))
|
||||
{
|
||||
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; //将当前的单元格设置为已到访
|
||||
F_open = PSDGetRaw(1) > DIST_cell; //获取前侧空间状况
|
||||
L_open = PSDGetRaw(2) > DIST_cell; //获取左侧空间状况
|
||||
R_open = PSDGetRaw(3) > 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();
|
||||
} //递归结束
|
||||
63
bot_code_turn/maze_func_flood.cpp
Normal file
63
bot_code_turn/maze_func_flood.cpp
Normal file
@@ -0,0 +1,63 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
void flood(int *map, int *copy_wall)
|
||||
{
|
||||
int num = 0;
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
map[rob_x0 * (size_y) + rob_y0] = 0;
|
||||
|
||||
do
|
||||
{
|
||||
num++;
|
||||
for (int i = 0; i < size_x; i++)
|
||||
{
|
||||
for (int j = 0; j < size_y; j++)
|
||||
{
|
||||
if (map[(i * size_y) + j] != -1)
|
||||
{
|
||||
if (i > 0)
|
||||
{
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子
|
||||
{
|
||||
map[((i-1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
//~ num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (i < size_x - 1)
|
||||
{
|
||||
if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子
|
||||
{
|
||||
map[((i+1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
//~ num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (j > 0)
|
||||
{
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1)
|
||||
{
|
||||
map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 1;
|
||||
//~ num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (j < size_y - 1)
|
||||
{
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1)
|
||||
{
|
||||
map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
|
||||
//~ num++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1 && num < (size_x * size_y));
|
||||
}
|
||||
95
bot_code_turn/maze_func_goto.cpp
Normal file
95
bot_code_turn/maze_func_goto.cpp
Normal file
@@ -0,0 +1,95 @@
|
||||
#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);
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
OSWait(800);
|
||||
|
||||
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向
|
||||
xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
|
||||
yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
|
||||
}
|
||||
60
bot_code_turn/maze_func_output.cpp
Normal file
60
bot_code_turn/maze_func_output.cpp
Normal file
@@ -0,0 +1,60 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---打印数组检查错误---*/
|
||||
/*---打印数组mark,map,copy_map等二维数组所用的函数---*/
|
||||
void output_arr2D(int size_x, int size_y, int *arr)
|
||||
{
|
||||
for (int i = size_y - 1; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j < size_x; j++)
|
||||
{
|
||||
cout << arr[(j * size_y) + i] << "\t";
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
/*---打印数组wall---*/
|
||||
void output_arrwall(int size_x, int size_y, int *arr)
|
||||
{
|
||||
for (int i = size_y; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j <= size_x; j++)
|
||||
{
|
||||
if (arr[(j * (size_y + 1) * 2) + (i * 2) + 1] == 1)
|
||||
{
|
||||
cout << "|" ;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << " " ;
|
||||
}
|
||||
|
||||
if (arr[(j * (size_y + 1) * 2) + (i * 2) + 0] == 1)
|
||||
{
|
||||
cout << "_" ;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << " " ;
|
||||
}
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
void output_arrpath(int size, int *arr)
|
||||
{
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
cout << arr[i] << " ";
|
||||
}
|
||||
cout << endl;
|
||||
cout << endl;
|
||||
}
|
||||
52
bot_code_turn/maze_func_path.cpp
Normal file
52
bot_code_turn/maze_func_path.cpp
Normal file
@@ -0,0 +1,52 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include<cstdlib>
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
/*---解迷宫路径到path数组函数---*/
|
||||
void build_path(int i, int j, int len, int *path, int *map, int *copy_wall)
|
||||
{
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
|
||||
for (int k = len -1; k >= 0; k--)
|
||||
{
|
||||
if (i > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == k) //前一个单位的东面是当前格，即当前格子的左侧为上一个格子
|
||||
{
|
||||
i--;
|
||||
path[k] = 3;
|
||||
}
|
||||
else if (i < size_x - 1 && !copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
|
||||
{
|
||||
i++;
|
||||
path[k] = 1;
|
||||
}
|
||||
else if(j > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i * size_y) + (j-1)] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
|
||||
{
|
||||
j--;
|
||||
path[k] = 0;
|
||||
}
|
||||
else if (j < size_y - 1 && !copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
|
||||
{
|
||||
j++;
|
||||
path[k] = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*---通过path数组移动到目标点函数---*/
|
||||
void drive_path(int len, int *path)
|
||||
{
|
||||
for(int i = 0; i < len; i++)
|
||||
{
|
||||
go_to(path[i]);
|
||||
}
|
||||
}
|
||||
142
bot_code_turn/maze_func_pid.cpp
Normal file
142
bot_code_turn/maze_func_pid.cpp
Normal file
@@ -0,0 +1,142 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
#include<cmath>
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---对PID控制的积分部分I限幅---*/
|
||||
void eI_xianfu()
|
||||
{
|
||||
if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
|
||||
{
|
||||
err_sum = eI_max;
|
||||
}
|
||||
else if (err_sum <= -eI_max)
|
||||
{
|
||||
err_sum = -eI_max;
|
||||
}
|
||||
else
|
||||
{
|
||||
//此处为空;
|
||||
}
|
||||
}
|
||||
|
||||
void pid_speed_xianfu()
|
||||
{
|
||||
if (Kpid_speed >= speed_xianfu)
|
||||
{
|
||||
Kpid_speed = speed_xianfu;
|
||||
}
|
||||
else if (Kpid_speed <= -speed_xianfu)
|
||||
{
|
||||
Kpid_speed = -speed_xianfu;
|
||||
}
|
||||
else
|
||||
{
|
||||
//此处为空
|
||||
}
|
||||
}
|
||||
|
||||
/*---位置式PID算法---*/
|
||||
void PID_AL()
|
||||
{
|
||||
err_sum += err; //积分累计误差值
|
||||
eI_xianfu(); //积分部分限幅
|
||||
err_diff = err - err_old; //微分部分误差值
|
||||
/*---打印输出PID控制的各项参数1---*/
|
||||
//~ cout << "err:" << err << " " ;
|
||||
//~ cout << "old_err:" << err_old << " " ;
|
||||
//~ cout << "err_sum:" << err_sum << " " ;
|
||||
//~ cout << "err_diff:" << err_diff << " " ;
|
||||
/*---打印结束---*/
|
||||
err_old = err; //将误差幅值到上一次误差
|
||||
|
||||
//~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
|
||||
//~ if (abs(err_sum) > 100 )
|
||||
//~ {
|
||||
Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base;
|
||||
//~ }
|
||||
//~ else
|
||||
//~ {
|
||||
//~ Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
|
||||
//~ }
|
||||
pid_speed_xianfu();
|
||||
//~ speed_r = speed_l - 1+ Kpid_speed;
|
||||
|
||||
|
||||
/*---打印输出PID控制的各项参数2---*/
|
||||
//~ cout << "PID_out:" << Kpid_speed << endl ;
|
||||
/*---打印结束---*/
|
||||
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
|
||||
|
||||
MOTORDriveRaw(2,speed_r + Kpid_speed);
|
||||
//~ MOTORDriveRaw(2,speed_r);
|
||||
MOTORDriveRaw(1,speed_l - Kpid_speed);
|
||||
//~ MOTORDriveRaw(1,speed_l);
|
||||
}
|
||||
|
||||
/*---PID控制下的直线行驶---*/
|
||||
void PIDStraight()
|
||||
{
|
||||
int bot_move; //定义基础角速度和速度,和移动距离
|
||||
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
|
||||
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
|
||||
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
|
||||
|
||||
//~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
|
||||
//~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
|
||||
//~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
|
||||
|
||||
MOTORDriveRaw(2,speed_r);
|
||||
MOTORDriveRaw(1,speed_l);
|
||||
|
||||
|
||||
do
|
||||
{
|
||||
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
|
||||
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
|
||||
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
|
||||
|
||||
//~ F_PSD = (PSDGetRaw(1) - 2) * 0.6 + F_PSD * 0.4; //读取前方和墙壁的距离
|
||||
//~ L_PSD = (PSDGetRaw(2) - 16) * 0.6 + L_PSD * 0.4; //读取左侧和墙壁的距离
|
||||
//~ R_PSD = (PSDGetRaw(3)) * 0.6 + R_PSD * 0.4; //读取右侧和墙壁的距离
|
||||
|
||||
/*---打印输出PSD数值---*/
|
||||
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
|
||||
/*---打印结束---*/
|
||||
|
||||
if (20<L_PSD && L_PSD<125 && 20<R_PSD && R_PSD<125) //如果两侧都有墙的情况
|
||||
{
|
||||
err = L_PSD - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else if (20<L_PSD && L_PSD<150) //只有左侧有墙的情况
|
||||
{
|
||||
err = L_PSD - DIST_wall_RL;
|
||||
PID_AL();
|
||||
}
|
||||
else if (20<R_PSD && R_PSD<150) //只有右侧有墙的情况
|
||||
{
|
||||
err = DIST_wall_RL - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else
|
||||
{
|
||||
//~ L_PSD = L_PSD % 200;
|
||||
//~ R_PSD = R_PSD % 200;
|
||||
//~ err = L_PSD - R_PSD;
|
||||
err = 0;
|
||||
err_old = 0;
|
||||
err_sum = 0;
|
||||
PID_AL();
|
||||
}
|
||||
VWGetPosition(&x_2, &y_2, &phi_2);
|
||||
bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
|
||||
}while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||||
//~ cout << "move:" << bot_move << endl;
|
||||
//~ MOTORDriveRaw(1,0);
|
||||
//~ MOTORDriveRaw(2,0);
|
||||
}
|
||||
58
bot_code_turn/maze_func_turn.cpp
Normal file
58
bot_code_turn/maze_func_turn.cpp
Normal file
@@ -0,0 +1,58 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
void BOTturn(int turn)
|
||||
{
|
||||
if (turn == -1) //右转
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,20);
|
||||
MOTORDriveRaw(2,-18);
|
||||
} while (abs(ENCODERRead(2)) < 240);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
else if (turn == 1) //左转
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,-20);
|
||||
MOTORDriveRaw(2,18);
|
||||
} while (abs(ENCODERRead(2)) < 240);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
else if (turn == 2) //掉头
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,20);
|
||||
MOTORDriveRaw(2,-18);
|
||||
|
||||
} while (abs(ENCODERRead(2)) < 570);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "errer" << endl;
|
||||
}
|
||||
err_sum = 0;
|
||||
err_old = 0;
|
||||
err = 0;
|
||||
err_diff = 0;
|
||||
|
||||
|
||||
}
|
||||
56
bot_code_turn/maze_main.cpp
Normal file
56
bot_code_turn/maze_main.cpp
Normal file
@@ -0,0 +1,56 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include<cstdlib>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---下面是主函数---*/
|
||||
int main()
|
||||
{
|
||||
explore(); //使用递归算法构建地图
|
||||
BOTturn(2 - rob_dir);
|
||||
VWWait();
|
||||
rob_dir = 2;
|
||||
explore();
|
||||
|
||||
const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度
|
||||
const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度
|
||||
|
||||
int copy_mark[mazesize_x * mazesize_y] = {}; //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0
|
||||
int copy_wall[(mazesize_x + 1) * (mazesize_y + 1) * 2] = {}; //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0
|
||||
|
||||
array_copy_mark(mazesize_x, mazesize_y, copy_mark); //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能
|
||||
array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
|
||||
|
||||
cout << "访问数组打印：" << endl;
|
||||
cout << endl;
|
||||
output_arr2D(mazesize_x, mazesize_y, copy_mark); //打印地图mark信息
|
||||
cout << "地图打印：" << endl;
|
||||
cout << endl;
|
||||
output_arrwall(mazesize_x, mazesize_y, copy_wall); //打印墙壁wall信息
|
||||
|
||||
cin_targey(); //输入目标点，带超界检测
|
||||
|
||||
int map[mazesize_x * mazesize_y] = {}; //创建最短路径求解地图并初始化为0
|
||||
array_negative_one(mazesize_x * mazesize_y, map); //将map数组初始化为-1
|
||||
|
||||
flood(map, copy_wall); //洪水填充算法
|
||||
cout << "路径打印：" << endl;
|
||||
cout << endl;
|
||||
output_arr2D(mazesize_x, mazesize_y, map); //打印map的数组信息
|
||||
|
||||
const int len = map[((target_x - 1) * mazesize_y) + (target_y -1)]; //迷宫的终点（人为确定）
|
||||
int path[len] = {}; //创建最短路径结果地图并初始化为0
|
||||
|
||||
build_path(target_x - 1, target_y - 1, len, path, map, copy_wall); //构建出最短路径path数组
|
||||
|
||||
cout << "移动指令打印：" << endl;
|
||||
cout << endl;
|
||||
output_arrpath(len, path); //打印path数组信息，及小车每一步的行驶方向
|
||||
|
||||
drive_path(len, path); //移动到指定目标点
|
||||
}
|
||||
59
bot_code_turn/maze_parameter.cpp
Normal file
59
bot_code_turn/maze_parameter.cpp
Normal file
@@ -0,0 +1,59 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"maze_parameter.h"
|
||||
// #include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
/*---定义数据---*/
|
||||
/*---定义全局常量---*/
|
||||
const int DIST_cell = 240; //单元格长度
|
||||
const int DIST_move = 320; //移动的单元格距离
|
||||
//~ const int DIST_move = 330; //移动的单元格距离
|
||||
const int DIST_wall_F = 100; //与墙壁的距离
|
||||
//~ 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坐标
|
||||
|
||||
int target_x; //声明目标点的X坐标
|
||||
int target_y; //声明目标点的Y坐标
|
||||
|
||||
/*---定义结束---*/
|
||||
47
bot_code_turn/maze_parameter.h
Normal file
47
bot_code_turn/maze_parameter.h
Normal file
@@ -0,0 +1,47 @@
|
||||
#pragma once
|
||||
#include<vector>
|
||||
|
||||
/*---声明数据---*/
|
||||
/*---声明全局常量---*/
|
||||
extern const int DIST_cell; //单元格长度
|
||||
extern const int DIST_move; //移动的单元格距离
|
||||
extern const int DIST_wall_F; //与前面墙壁的距离
|
||||
extern const int DIST_wall_RL; //与左右墙壁的距离
|
||||
|
||||
/*---声明全局变量---*/
|
||||
|
||||
/*---声明全局容器---*/
|
||||
extern std::vector<std::vector<int>> mark; //声明mark容器（二维容器）
|
||||
extern std::vector<std::vector<std::vector<int>>> wall; //声明wall容器（三维容器）
|
||||
|
||||
/*---声明机器人坐标---*/
|
||||
extern int rob_x0; //机器人起点X坐标
|
||||
extern int rob_y0; //机器人起点Y坐标
|
||||
extern int rob_x; //机械人当前X坐标
|
||||
extern int rob_y; //机器人当前Y坐标
|
||||
extern int rob_dir; //机器人当前的朝向
|
||||
|
||||
/*---各种全局量声明结束---*/
|
||||
|
||||
/*---递归算法---*/
|
||||
|
||||
/*---PID算法---*/
|
||||
extern const float Kp, Ki, Kd; //声明PID的参数
|
||||
//~ extern float Kpid; //定义PID计算后输出的倍率
|
||||
extern int Kpid_speed; //定义计算后的速度
|
||||
extern const float Kpid_base; //定义速度基础倍数速度
|
||||
extern const int speed_l; //定义左轮基本速度
|
||||
extern const int speed_r; //定义右轮基本速度
|
||||
//~ extern int speed_r; //定义右轮基本速度
|
||||
extern const int speed_xianfu; //kpid输出速度的限幅
|
||||
extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||||
extern const int eI_max; //定义积分限幅
|
||||
|
||||
/*---目标点坐标---*/
|
||||
// extern const int target_x; //声明目标点的X坐标
|
||||
// extern const int target_y; //声明目标点的Y坐标
|
||||
|
||||
extern int target_x; //声明目标点的X坐标
|
||||
extern int target_y; //声明目标点的Y坐标
|
||||
|
||||
/*---声明结束---*/
|
||||
16
bot_code_turn/全局变量.txt
Normal file
16
bot_code_turn/全局变量.txt
Normal file
@@ -0,0 +1,16 @@
|
||||
几个数组：
|
||||
wall，mark，maze....
|
||||
|
||||
机器人坐标：
|
||||
rob_x 机器人X坐标
|
||||
rob_y 机器人Y坐标
|
||||
rob_dir 机器人朝向
|
||||
|
||||
迷宫基础数据：
|
||||
DIST_cell 单个单元格的长度
|
||||
DIST_wall PID直线的时候，检测与墙壁的距离值
|
||||
|
||||
PID：
|
||||
err，err_old，err_sum，err_diff, eI_max 误差，上一次误差，积分累计误差，微分误差变化，积分限幅
|
||||
Kp, Ki, Kd, Kpid PID控制的参数P,I,D以及算法输出的倍率参数
|
||||
|
||||
5
bot_code_turn/名称解释.txt
Normal file
5
bot_code_turn/名称解释.txt
Normal file
@@ -0,0 +1,5 @@
|
||||
头文件:mazetool.h
|
||||
函数文件:mazetool.cpp
|
||||
全局:
|
||||
rob_y:机器人在迷宫地图中的Y坐标值
|
||||
rob_x:机器人在迷宫地图中的x坐标值
|
||||
@@ -20,7 +20,7 @@ void go_to(int dir); //行驶到指定单元格
|
||||
|
||||
/*---PART3---*/
|
||||
/*---以下为洪水填充算法部分涉及到的函数声明---*/
|
||||
void flood(int *map, int *copy_map, int *copy_wall); //洪水填充算法
|
||||
void flood(int *map, int *copy_wall); //洪水填充算法
|
||||
/*---以上为洪水填充算法部分涉及到的函数声明---*/
|
||||
|
||||
/*---PART4---*/
|
||||
@@ -31,7 +31,7 @@ void drive_path(int len, int *path); //移动到目的地
|
||||
|
||||
/*---PART5---*/
|
||||
/*---以下为数组构建部分涉及到的函数声明---*/
|
||||
void array_negative_one(int *arr); //将数组初始化为-1
|
||||
void array_negative_one(int size, int *arr); //将数组初始化为-1
|
||||
void array_copy_mark(int size_x, int size_y, int *copy_mark); //复制一份mark数组，便于后续频繁读取提升性能
|
||||
void array_copy_wall(int size_x, int size_y, int *copy_wall); //复制一份wall数组，便于后续频繁读取提升性能
|
||||
/*---以上为数组构建部分涉及到的函数声明---*/
|
||||
@@ -40,6 +40,6 @@ void array_copy_wall(int size_x, int size_y, int *copy_wall); //复制一份wa
|
||||
/*---以下为数组打印部分涉及到的函数声明---*/
|
||||
void output_arr2D(int size_x, int size_y, int *arr); //打印所有二维数组数组
|
||||
void output_arrwall(int size_x, int size_y, int *arr); //打印wall数组
|
||||
void output_arrpath(int *arr); //打印path数组
|
||||
void output_arrpath(int size, int *arr); //打印path数组
|
||||
/*---以上为数组打印部分涉及到的函数声明---*/
|
||||
|
||||
|
||||
@@ -27,17 +27,14 @@ void array_copy_wall(int size_x, int size_y, int *copy_wall)
|
||||
{
|
||||
for(int k = 0; k < 2; k++)
|
||||
{
|
||||
copy_wall[i * size_y * 2 + j * 2 + k] = wall[i][j][k];
|
||||
copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k]; //注释：k = 0记录的是单元格下方的数据，1记录的是单元格左边的数据，所以转为一维数组后，i*...+j*...+k,k=1为左侧数据，k=0为下方墙壁。
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*---将二维数组初始化为-1---*/
|
||||
void array_negative_one(int *arr)
|
||||
void array_negative_one(int size, int *arr) //将数组初始化为-1
|
||||
{
|
||||
int size = sizeof(arr) / sizeof(arr[0]); //获取X轴长度
|
||||
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
arr[i] = -1;
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include<cstddef>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
@@ -21,89 +22,40 @@ bool check_mark()
|
||||
bool S_mark;
|
||||
bool E_mark;
|
||||
|
||||
if (x>0 && y>0) //情况1
|
||||
if (x > 0)
|
||||
{
|
||||
if (y == size_y - 1) //N
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
W_mark = mark[x-1][y]; //W
|
||||
S_mark = mark[x][y-1]; //S
|
||||
if (x == size_x -1) //E
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
W_mark = mark[x-1][y];
|
||||
}
|
||||
else if (x==0 && y>0) //情况2
|
||||
else
|
||||
{
|
||||
if (y == size_y - 1) //N
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
W_mark = true; //W
|
||||
S_mark = mark[x][y-1]; //S
|
||||
if (x == size_x - 1) //E
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
W_mark = true;
|
||||
}
|
||||
else if (x>0 && y==0) //情况3
|
||||
|
||||
if (y > 0)
|
||||
{
|
||||
if (y == size_y - 1) //N
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
W_mark = mark[x-1][y]; //W
|
||||
S_mark = true; //S
|
||||
if (x == size_x -1) //E
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
S_mark = mark[x][y-1];
|
||||
}
|
||||
else //(x==0 && y==0)
|
||||
else
|
||||
{
|
||||
if (y == size_y - 1) //N
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
W_mark = true; //W
|
||||
S_mark = true; //S
|
||||
if (x == size_x -1) //E
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
S_mark = true;
|
||||
}
|
||||
|
||||
if (x < size_x - 1)
|
||||
{
|
||||
E_mark = mark[x+1][y];
|
||||
}
|
||||
else
|
||||
{
|
||||
E_mark = true;
|
||||
}
|
||||
|
||||
if (y < size_y - 1)
|
||||
{
|
||||
N_mark = mark[x][y+1];
|
||||
}
|
||||
else
|
||||
{
|
||||
N_mark = true;
|
||||
}
|
||||
|
||||
bool N_road = (!wall[x][y+1][0] && N_mark);
|
||||
@@ -126,15 +78,22 @@ void maze_entry(int x, int y, int dir, int open)
|
||||
switch (maze_dir)
|
||||
{
|
||||
case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
|
||||
if (y == mark[0].size() - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
|
||||
if (y == static_cast<int>(mark[0].size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
|
||||
{
|
||||
for (int i = 0; i < mark.size() - 1; i++)
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
mark[i].emplace_back(0);
|
||||
for (size_t i = 0; i < mark.size(); i++)
|
||||
{
|
||||
mark[i].emplace_back(0);
|
||||
}
|
||||
for (size_t i = 0; i < wall.size(); i++)
|
||||
{
|
||||
wall[i].emplace_back(std::vector<int>(2,0));
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < wall.size() - 1; i++)
|
||||
else
|
||||
{
|
||||
wall[i].emplace_back(vector<int>(2,0));
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x][y+1][0] = !open;
|
||||
@@ -143,8 +102,21 @@ void maze_entry(int x, int y, int dir, int open)
|
||||
case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
|
||||
if (x == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层
|
||||
{
|
||||
mark.emplace(mark.begin(), mark[0].size(), 0);
|
||||
wall.emplace(wall.begin(), wall[0].size(), vector<int>(2,0));
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
// std::vector<int> mark_insert(mark[0].size(), 0);
|
||||
// mark.insert(mark.begin(), mark_insert);
|
||||
|
||||
// std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
|
||||
// wall.insert(wall.begin(), wall_insert);
|
||||
|
||||
mark.emplace(mark.begin(), std::vector<int>(mark[0].size(), 0)); //在mark头部插入一个内层
|
||||
wall.emplace(wall.begin(), std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0))); //在wall头部插入一个内层
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x][y][1] = !open;
|
||||
break;
|
||||
@@ -152,23 +124,37 @@ void maze_entry(int x, int y, int dir, int open)
|
||||
case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
|
||||
if (y == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据
|
||||
{
|
||||
for (int i = 0; i < mark.size() - 1; i++)
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
mark[i].emplace(mark[i].begin(), 0);
|
||||
for (size_t i = 0; i < mark.size(); i++)
|
||||
{
|
||||
mark[i].emplace(mark[i].begin(), 0);
|
||||
}
|
||||
for (size_t i = 0; i < wall.size(); i++)
|
||||
{
|
||||
wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < wall.size() - 1; i++)
|
||||
else
|
||||
{
|
||||
wall[i].emplace(wall[i].begin(), vector<int>(2,0));
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x][y][0] = !open;
|
||||
break;
|
||||
|
||||
case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息
|
||||
if (x == (mark.size() - 1) && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
|
||||
if (x == static_cast<int>(mark.size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
|
||||
{
|
||||
mark.emplace_back(mark[0].size(), 0);
|
||||
wall.emplace_back(wall[0].size(), vector<int>(2,0));
|
||||
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
|
||||
{
|
||||
mark.emplace_back(std::vector<int>(mark[0].size(), 0));
|
||||
wall.emplace_back(std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0)));
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "error: mark or wall empty" << endl;
|
||||
}
|
||||
}
|
||||
wall[x+1][y][1] = !open;
|
||||
break;
|
||||
@@ -183,7 +169,7 @@ bool unmarked(int x, int y, int dir)
|
||||
switch(maze_dir)
|
||||
{
|
||||
case 0:
|
||||
if (y < mark[0].size() - 2)
|
||||
if (y < (static_cast<int>(mark[0].size()) - 1))
|
||||
{
|
||||
if (mark[x][y+1] == 0)
|
||||
{
|
||||
@@ -193,7 +179,7 @@ bool unmarked(int x, int y, int dir)
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (x > 1)
|
||||
if (x > 0)
|
||||
{
|
||||
if (mark[x-1][y] == 0)
|
||||
{
|
||||
@@ -203,7 +189,7 @@ bool unmarked(int x, int y, int dir)
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if (y > 1)
|
||||
if (y > 0)
|
||||
{
|
||||
if (mark[x][y-1] == 0)
|
||||
{
|
||||
@@ -213,7 +199,7 @@ bool unmarked(int x, int y, int dir)
|
||||
break;
|
||||
|
||||
case 3:
|
||||
if (x < mark.size() - 2)
|
||||
if (x < (static_cast<int>(mark.size()) - 1))
|
||||
{
|
||||
if (mark[x+1][y] == 0)
|
||||
{
|
||||
|
||||
@@ -6,13 +6,12 @@
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
void flood(int *map, int *copy_map, int *copy_wall)
|
||||
void flood(int *map, int *copy_wall)
|
||||
{
|
||||
int num = 0;
|
||||
int size_x = mark.size();
|
||||
int size_y = mark[0].size();
|
||||
map[0] = 0;
|
||||
copy_map[0] = 0;
|
||||
|
||||
do
|
||||
{
|
||||
@@ -24,49 +23,41 @@ void flood(int *map, int *copy_map, int *copy_wall)
|
||||
{
|
||||
if (i > 0)
|
||||
{
|
||||
if (!copy_wall[(i * size_y * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子
|
||||
{
|
||||
copy_map[((i-1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
map[((i-1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (i < size_x - 1)
|
||||
{
|
||||
if (!copy_wall[((i+1) * size_y * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子
|
||||
if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子
|
||||
{
|
||||
copy_map[((i+1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
map[((i+1) * size_y) + j] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (j > 0)
|
||||
{
|
||||
if (!copy_wall[(i * size_y * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1)
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1)
|
||||
{
|
||||
copy_map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 1;
|
||||
map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
if (j < size_y - 1)
|
||||
{
|
||||
if (!copy_wall[(i * size_y * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1)
|
||||
if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1)
|
||||
{
|
||||
copy_map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
|
||||
map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < size_x; i++)
|
||||
{
|
||||
for (int j = 0; j < size_y; j++)
|
||||
{
|
||||
map[(i * size_y) + j] = copy_map[(i * size_y) + j];
|
||||
}
|
||||
}
|
||||
} while (map[(target_x * size_y) + target_y] == -1 && num < (size_x * size_y));
|
||||
} while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1 && num < (size_x * size_y));
|
||||
}
|
||||
@@ -1,6 +1,5 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
@@ -11,7 +9,6 @@ using namespace std;
|
||||
/*---打印数组mark,map,copy_map等二维数组所用的函数---*/
|
||||
void output_arr2D(int size_x, int size_y, int *arr)
|
||||
{
|
||||
|
||||
for (int i = size_y - 1; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j < size_x; j++)
|
||||
@@ -25,12 +22,11 @@ void output_arr2D(int size_x, int size_y, int *arr)
|
||||
/*---打印数组wall---*/
|
||||
void output_arrwall(int size_x, int size_y, int *arr)
|
||||
{
|
||||
|
||||
for (int i = size_y - 1; i >= 0; i--)
|
||||
for (int i = size_y; i >= 0; i--)
|
||||
{
|
||||
for (int j = 0; j < size_x; j++)
|
||||
for (int j = 0; j <= size_x; j++)
|
||||
{
|
||||
if (arr[(j * size_y * 2) + (i * 2) + 1] == 1)
|
||||
if (arr[(j * (size_y + 1) * 2) + (i * 2) + 1] == 1)
|
||||
{
|
||||
cout << "|" ;
|
||||
}
|
||||
@@ -39,7 +35,7 @@ void output_arrwall(int size_x, int size_y, int *arr)
|
||||
cout << " " ;
|
||||
}
|
||||
|
||||
if (arr[(j * size_y * 2) + (i * 2) + 0] == 1)
|
||||
if (arr[(j * (size_y + 1) * 2) + (i * 2) + 0] == 1)
|
||||
{
|
||||
cout << "_" ;
|
||||
}
|
||||
@@ -52,11 +48,8 @@ void output_arrwall(int size_x, int size_y, int *arr)
|
||||
}
|
||||
}
|
||||
|
||||
/*---打印数组path---*/
|
||||
void output_arrpath(int *arr)
|
||||
void output_arrpath(int size, int *arr)
|
||||
{
|
||||
int size = sizeof(arr) / sizeof(arr[0]);
|
||||
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
cout << arr[i] << " ";
|
||||
|
||||
@@ -14,22 +14,22 @@ void build_path(int i, int j, int len, int *path, int *map, int *copy_wall)
|
||||
|
||||
for (int k = len -1; k >= 0; k--)
|
||||
{
|
||||
if (i > 0 && !copy_wall[(i * size_y * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == k) //前一个单位的东面是当前格，即当前格子的左侧为上一个格子
|
||||
if (i > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == k) //前一个单位的东面是当前格，即当前格子的左侧为上一个格子
|
||||
{
|
||||
i--;
|
||||
path[k] = 3;
|
||||
}
|
||||
else if (i < size_x - 1 && !copy_wall[((i+1) * size_y * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
|
||||
else if (i < size_x - 1 && !copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
|
||||
{
|
||||
i++;
|
||||
path[k] = 1;
|
||||
}
|
||||
else if(j > 0 && !copy_wall[(i * size_y * 2) + (j * 2) + 0] && map[(i * size_y) + (j-1)] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
|
||||
else if(j > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i * size_y) + (j-1)] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
|
||||
{
|
||||
j--;
|
||||
path[k] = 0;
|
||||
}
|
||||
else if (j < size_y - 1 && !copy_wall[(i * size_y * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
|
||||
else if (j < size_y - 1 && !copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
|
||||
{
|
||||
j++;
|
||||
path[k] = 2;
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
@@ -31,21 +30,21 @@ void PID_AL()
|
||||
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" ;
|
||||
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控制算法输出的倍率
|
||||
Kpid_speed = Kpid * Kpid_base;
|
||||
/*---打印输出PID控制的各项参数2---*/
|
||||
// cout << "PID输出值:" << Kpid << endl ;
|
||||
cout << "PID输出值:" << Kpid_speed << "\t" ;
|
||||
/*---打印结束---*/
|
||||
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
|
||||
MOTORDriveRaw(1,speed1);
|
||||
MOTORDriveRaw(2,speed1+Kpid_speed);
|
||||
MOTORDriveRaw(2,speed1 + Kpid_speed);
|
||||
}
|
||||
|
||||
/*---PID控制下的直线行驶---*/
|
||||
@@ -70,19 +69,19 @@ void PIDStraight()
|
||||
// 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) //如果两侧都有墙的情况
|
||||
if (30<L_PSD && L_PSD<180 && 30<R_PSD && R_PSD<180) //如果两侧都有墙的情况
|
||||
{
|
||||
err = L_PSD - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else if (100<L_PSD && L_PSD<180) //只有左侧有墙的情况
|
||||
else if (30<L_PSD && L_PSD<180) //只有左侧有墙的情况
|
||||
{
|
||||
err = L_PSD - DIST_wall;
|
||||
err = L_PSD - DIST_wall_RL;
|
||||
PID_AL();
|
||||
}
|
||||
else if (100<R_PSD && R_PSD<180) //只有右侧有墙的情况
|
||||
else if (30<R_PSD && R_PSD<180) //只有右侧有墙的情况
|
||||
{
|
||||
err = DIST_wall - R_PSD;
|
||||
err = DIST_wall_RL - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else
|
||||
@@ -94,9 +93,12 @@ void PIDStraight()
|
||||
}
|
||||
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的时候降点速度
|
||||
{
|
||||
speed1 = speed2;
|
||||
}
|
||||
}while (DIST_move < DIST_cell && F_PSD > DIST_wall); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||||
cout << "move:" << DIST_move << endl;
|
||||
// if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
|
||||
// {
|
||||
// speed1 = speed2;
|
||||
// }
|
||||
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
|
||||
@@ -11,7 +11,8 @@ using namespace std;
|
||||
int main()
|
||||
{
|
||||
explore(); //使用递归算法构建地图
|
||||
|
||||
wall[0][0][0] = 1;
|
||||
|
||||
const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度
|
||||
const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度
|
||||
|
||||
@@ -22,24 +23,20 @@ int main()
|
||||
array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
|
||||
|
||||
output_arr2D(mazesize_x, mazesize_y, copy_mark); //打印地图mark信息
|
||||
output_arrwall(mazesize_x + 1, mazesize_y + 1, copy_wall); //打印墙壁wall信息
|
||||
output_arrwall(mazesize_x, mazesize_y, copy_wall); //打印墙壁wall信息
|
||||
|
||||
int map[mazesize_x * mazesize_y] = {}; //创建最短路径求解地图并初始化为0
|
||||
int copy_map[mazesize_x * mazesize_y] = {}; //创建copy复制地图并初始化为0
|
||||
array_negative_one(mazesize_x * mazesize_y, map); //将map数组初始化为-1
|
||||
|
||||
array_negative_one(map); //将map数组初始化为-1
|
||||
array_negative_one(copy_map); //将copy_map数组初始化为-1
|
||||
|
||||
flood(map, copy_map, copy_wall); //洪水填充算法
|
||||
|
||||
flood(map, copy_wall); //洪水填充算法
|
||||
output_arr2D(mazesize_x, mazesize_y, map); //打印map的数组信息
|
||||
|
||||
const int len = map[(target_x * mazesize_y) + target_y]; //迷宫的终点（人为确定）
|
||||
const int len = map[((target_x - 1) * mazesize_y) + (target_y -1)]; //迷宫的终点（人为确定）
|
||||
int path[len] = {}; //创建最短路径结果地图并初始化为0
|
||||
|
||||
build_path(target_x, target_y, len, path, map, copy_wall); //构建出最短路径path数组
|
||||
build_path(target_x - 1, target_y - 1, len, path, map, copy_wall); //构建出最短路径path数组
|
||||
drive_path(len, path); //移动到指定目标点
|
||||
|
||||
output_arrpath(path); //打印path数组信息，及小车每一步的行驶方向
|
||||
output_arrpath(len, path); //打印path数组信息，及小车每一步的行驶方向
|
||||
|
||||
}
|
||||
|
||||
@@ -3,18 +3,19 @@
|
||||
#include<vector>
|
||||
#include"eyebot++.h"
|
||||
#include"maze_parameter.h"
|
||||
#include"maze_func.h"
|
||||
// #include"maze_func.h"
|
||||
using namespace std;
|
||||
|
||||
/*---定义数据---*/
|
||||
/*---定义全局常量---*/
|
||||
const int DIST_cell = 360 ; //单元格长度
|
||||
const int DIST_wall = 130 ; //与墙壁的距离
|
||||
const int DIST_cell = 300 ; //单元格长度
|
||||
const int DIST_wall_F = 90 ; //与墙壁的距离
|
||||
extern const int DIST_wall_RL = 100; //与左右墙壁的距离
|
||||
|
||||
/*---定义全局变量---*/
|
||||
/*---容器类---*/
|
||||
vector<vector<int>> mark(1,vector<int>(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问
|
||||
vector<vector<vector<int>>> wall(1,vector<vector<int>>(1,vector<int>(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路
|
||||
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坐标
|
||||
@@ -27,11 +28,12 @@ int rob_dir = 0; //机器人当前的朝向（初始为0）
|
||||
|
||||
|
||||
/*---PID算法---*/
|
||||
const float Kp = 1, Ki = 0, Kd = 0.5; //定义PID的参数
|
||||
const float Kp = 0.5, Ki = 0.2, Kd = 0.5; //定义PID的参数
|
||||
float Kpid; //定义PID计算后输出的倍率
|
||||
float Kpid_speed;
|
||||
const int Kpid_base = 2, speed = 24; //定义基本角速度和速度
|
||||
const int speed2 = 0.75*speed; //快到目的地时减速
|
||||
int Kpid_speed;
|
||||
const float Kpid_base = 0.1; //定义基本角速度和速度
|
||||
const int speed = 50; //定义基本角速度和速度
|
||||
const int speed2 = 0.75*speed; //快到目的地时减速
|
||||
int speed1;
|
||||
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||||
const int eI_max = 20; //定义积分限幅
|
||||
|
||||
@@ -4,13 +4,14 @@
|
||||
/*---声明数据---*/
|
||||
/*---声明全局常量---*/
|
||||
extern const int DIST_cell; //单元格长度
|
||||
extern const int DIST_wall; //与墙壁的距离
|
||||
extern const int DIST_wall_F; //与前面墙壁的距离
|
||||
extern const int DIST_wall_RL; //与左右墙壁的距离
|
||||
|
||||
/*---声明全局变量---*/
|
||||
|
||||
/*---声明全局容器---*/
|
||||
extern std::vector<std::vector<int>> mark; //声明mark容器（二维容器）
|
||||
extern std::vector<std::vector<std::vector<int>>> wall; //声明wall容器（三维容器）
|
||||
extern std::vector<std::vector<int>> mark; //声明mark容器（二维容器）
|
||||
extern std::vector<std::vector<std::vector<int>>> wall; //声明wall容器（三维容器）
|
||||
|
||||
/*---声明机器人坐标---*/
|
||||
extern int rob_x0; //机器人起点X坐标
|
||||
@@ -26,8 +27,9 @@ extern int rob_dir; //机器人当前的朝向
|
||||
/*---PID算法---*/
|
||||
extern const float Kp, Ki, Kd; //声明PID的参数
|
||||
extern float Kpid; //定义PID计算后输出的倍率
|
||||
extern float Kpid_speed; //定义计算后的速度
|
||||
extern const int Kpid_base, speed; //定义基本角速度和速度
|
||||
extern int Kpid_speed; //定义计算后的速度
|
||||
extern const float Kpid_base; //定义速度基础倍数速度
|
||||
extern const int speed; //定义速度基础倍数速度
|
||||
extern const int speed2; //快到目的地时减速
|
||||
extern int speed1;
|
||||
extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||||
|
||||
10
robot_test_code/LCD输出测试/lcd_test1.cpp
Normal file
10
robot_test_code/LCD输出测试/lcd_test1.cpp
Normal file
@@ -0,0 +1,10 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
}
|
||||
20
robot_test_code/LCD输出测试/makefile
Normal file
20
robot_test_code/LCD输出测试/makefile
Normal file
@@ -0,0 +1,20 @@
|
||||
|
||||
cxx := gccarm
|
||||
|
||||
file_cpp := $(wildcard *.cpp)
|
||||
file_o := /home/pi/usr/pid.x
|
||||
|
||||
$(file_o) : $(file_cpp)
|
||||
@$(cxx) $^ -o $@
|
||||
|
||||
maze : $(file_o)
|
||||
|
||||
clean :
|
||||
@rm -rf $(file_o)
|
||||
|
||||
debug :
|
||||
@echo $(cxx)
|
||||
@echo $(file_cpp)
|
||||
@echo $(file_o)
|
||||
|
||||
.PHONY : maze clean debug
|
||||
168
robot_test_code/PID/PID.cpp
Normal file
168
robot_test_code/PID/PID.cpp
Normal file
@@ -0,0 +1,168 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
#include"PID.h"
|
||||
using namespace std;
|
||||
|
||||
float Kp = 0.815, Ki = 0.0083, Kd = 0.167; //定义PID的参数
|
||||
float Kpid;
|
||||
int Kpid_speed;
|
||||
const int speed_l = 18;
|
||||
const int speed_r = speed_l - 2;
|
||||
const int speed_err = 2;
|
||||
const float Kpid_base = 1;
|
||||
|
||||
/*---定义全局变量---*/
|
||||
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||||
const int eI_max = 200;
|
||||
/*---定义全局常量---*/
|
||||
/*---定义全局常量---*/
|
||||
const int DIST_cell = 310;
|
||||
const int DIST_wall_f = 75;
|
||||
const int DIST_wall_rl = 90;
|
||||
/*---全局各种变量与常量定义完成---*/
|
||||
|
||||
|
||||
/*---对PID控制的积分部分I限幅---*/
|
||||
void eI_xianfu(int eI_max)
|
||||
{
|
||||
if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
|
||||
{
|
||||
err_sum = eI_max;
|
||||
}
|
||||
else if (err_sum <= -eI_max)
|
||||
{
|
||||
err_sum = -eI_max;
|
||||
}
|
||||
else
|
||||
{
|
||||
//此处为空;
|
||||
}
|
||||
}
|
||||
|
||||
/*---位置式PID算法---*/
|
||||
void PID_AL()
|
||||
{
|
||||
err_sum += err; //积分累计误差值
|
||||
eI_xianfu(eI_max); //积分部分限幅
|
||||
err_diff = err - err_old; //微分部分误差值
|
||||
/*---打印输出PID控制的各项参数1---*/
|
||||
cout << "err:" << err << " " ;
|
||||
cout << "err_old:" << err_old << " " ;
|
||||
cout << "err_sum:" << err_sum << " " ;
|
||||
cout << "err_diff:" << err_diff << " " ;
|
||||
/*---打印结束---*/
|
||||
err_old = err; //将误差幅值到上一次误差
|
||||
|
||||
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
|
||||
Kpid_speed = Kpid * Kpid_base;
|
||||
if (Kpid_speed >= speed_err) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
|
||||
{
|
||||
Kpid_speed = speed_err;
|
||||
}
|
||||
else if (Kpid_speed <= -speed_err)
|
||||
{
|
||||
Kpid_speed = -speed_err;
|
||||
}
|
||||
else
|
||||
{
|
||||
//error
|
||||
}
|
||||
/*---打印输出PID控制的各项参数2---*/
|
||||
cout << "PID_out:" << Kpid_speed << " " ;
|
||||
/*---打印结束---*/
|
||||
|
||||
MOTORDriveRaw(2,Kpid_speed + speed_r); //控制小车的行驶速度，角速度
|
||||
MOTORDriveRaw(1,speed_l);
|
||||
|
||||
}
|
||||
|
||||
/*---PID控制下的直线行驶---*/
|
||||
void PIDStraight()
|
||||
{
|
||||
int DIST_move; //定义基础角速度和速度,和移动距离
|
||||
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
|
||||
//~ int DIST_move_x1;
|
||||
//~ int DIST_move_x2;
|
||||
//~ int L_PSD_old, R_PSD_old, F_PSD_old;
|
||||
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
|
||||
// speed1 = speed;
|
||||
|
||||
//~ F_PSD = PSDGetRaw(1);
|
||||
//~ F_PSD_old = F_PSD;
|
||||
|
||||
//~ L_PSD = PSDGetRaw(2) - 8;
|
||||
//~ L_PSD_old = L_PSD;
|
||||
|
||||
//~ R_PSD = PSDGetRaw(3);
|
||||
//~ R_PSD_old = R_PSD;
|
||||
|
||||
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
|
||||
//~ DIST_move_x1 = F_PSD;
|
||||
|
||||
MOTORDriveRaw(2,speed_r);
|
||||
MOTORDriveRaw(1,speed_l);
|
||||
|
||||
|
||||
do
|
||||
{
|
||||
F_PSD = PSDGetRaw(1) - 5; //读取前方和墙壁的距离
|
||||
//~ F_PSD = PSDGetRaw(1) * 0.3 + F_PSD_old * 0.7;
|
||||
//~ F_PSD_old = F_PSD;
|
||||
|
||||
L_PSD = PSDGetRaw(2) - 10; //读取左侧和墙壁的距离
|
||||
//~ L_PSD = PSDGetRaw(2) * 0.3 + L_PSD_old * 0.7;
|
||||
//~ L_PSD_old = L_PSD;
|
||||
|
||||
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
|
||||
//~ R_PSD = PSDGetRaw(3) * 0.3 + R_PSD_old * 0.7;
|
||||
//~ R_PSD_old = R_PSD;
|
||||
|
||||
/*---打印输出PSD数值---*/
|
||||
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
|
||||
/*---打印结束---*/
|
||||
if (20<L_PSD && L_PSD<180 && 20<R_PSD && R_PSD<180) //如果两侧都有墙的情况
|
||||
{
|
||||
err = L_PSD - R_PSD;
|
||||
PID_AL();
|
||||
}
|
||||
else if (20<L_PSD && L_PSD<180) //只有左侧有墙的情况
|
||||
{
|
||||
err = L_PSD - DIST_wall_rl;
|
||||
PID_AL();
|
||||
}
|
||||
else if (20<R_PSD && R_PSD<180) //只有右侧有墙的情况
|
||||
{
|
||||
err = DIST_wall_rl - 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));
|
||||
|
||||
//~ DIST_move_x2 = F_PSD;
|
||||
//~ DIST_move = DIST_move_x1 - DIST_move_x2;
|
||||
|
||||
cout << "move: " << DIST_move << endl;
|
||||
|
||||
|
||||
// if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
|
||||
// {
|
||||
// speed1 = speed2;
|
||||
// }
|
||||
|
||||
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||||
//~ }while (F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||||
//~ MOTORDriveRaw(1,-15);
|
||||
//~ MOTORDriveRaw(2,-15);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
//VWSetSpeed(0,0);
|
||||
}
|
||||
5
robot_test_code/PID/PID.h
Normal file
5
robot_test_code/PID/PID.h
Normal file
@@ -0,0 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
void eI_xianfu(int eI_max); //积分部分限幅
|
||||
void PID_AL(); //位置式PID算法
|
||||
void PIDStraight(); //PID控制下的直线行驶
|
||||
69
robot_test_code/PID/PIDmain.cpp
Normal file
69
robot_test_code/PID/PIDmain.cpp
Normal file
@@ -0,0 +1,69 @@
|
||||
#include<iostream>
|
||||
#include"PID.h"
|
||||
#include"eyebot++.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
/*---定义全局常量---*/
|
||||
// const int DIST_cell = 300 ;
|
||||
// const int DIST_wall = 100 ;
|
||||
const int DIST_cell = 310;
|
||||
const int DIST_wall_f = 75;
|
||||
const int DIST_wall_rl = 90;
|
||||
/*---全局各种变量与常量定义完成---*/
|
||||
|
||||
|
||||
/*---下面是主函数---*/
|
||||
int main()
|
||||
{
|
||||
//~ int Fwall,Lwall,Rwall;
|
||||
int F_wall;
|
||||
LCDMenu("","","","END");
|
||||
int num;
|
||||
|
||||
do
|
||||
{ //检测墙面
|
||||
|
||||
//~ Fwall = PSDGetRaw(1) > DIST_cell; //检测前面侧是否有墙
|
||||
//~ Lwall = PSDGetRaw(2) > DIST_cell; //检测左面
|
||||
//~ Rwall = PSDGetRaw(3) > DIST_cell; //检测右面
|
||||
F_wall = PSDGetRaw(1);
|
||||
|
||||
if (F_wall < DIST_wall_f)
|
||||
{
|
||||
goto end_main;
|
||||
}
|
||||
//~ //判断旋转方向
|
||||
//~ if (Lwall)
|
||||
//~ {
|
||||
//~ VWTurn(+90,15); //向左旋转
|
||||
//~ VWWait();
|
||||
//~ }
|
||||
//~ else if (Fwall)
|
||||
//~ {
|
||||
//~ //不旋转
|
||||
//~ }
|
||||
//~ else if (Rwall)
|
||||
//~ {
|
||||
//~ VWTurn(-90,15); //向右旋转
|
||||
//~ VWWait();
|
||||
//~ }
|
||||
//~ else
|
||||
//~ {
|
||||
//~ VWTurn(180,15);
|
||||
//~ VWWait();
|
||||
//~ }
|
||||
//~ //直走
|
||||
|
||||
cin >> num ;
|
||||
if (num)
|
||||
{
|
||||
PIDStraight();
|
||||
}
|
||||
//~ PIDStraight();
|
||||
VWWait();
|
||||
|
||||
} while (KEYRead() != KEY4);
|
||||
end_main:
|
||||
cout << "over" << endl;
|
||||
}
|
||||
20
robot_test_code/PID/makefile
Normal file
20
robot_test_code/PID/makefile
Normal file
@@ -0,0 +1,20 @@
|
||||
|
||||
cxx := gccarm
|
||||
|
||||
file_cpp := $(wildcard *.cpp)
|
||||
file_o := /home/pi/usr/pid.x
|
||||
|
||||
$(file_o) : $(file_cpp)
|
||||
@$(cxx) $^ -o $@
|
||||
|
||||
maze : $(file_o)
|
||||
|
||||
clean :
|
||||
@rm -rf $(file_o)
|
||||
|
||||
debug :
|
||||
@echo $(cxx)
|
||||
@echo $(file_cpp)
|
||||
@echo $(file_o)
|
||||
|
||||
.PHONY : maze clean debug
|
||||
28
robot_test_code/实机测试/makefile
Normal file
28
robot_test_code/实机测试/makefile
Normal file
@@ -0,0 +1,28 @@
|
||||
|
||||
cxx := gccarm
|
||||
#~ cxx := gpparm
|
||||
|
||||
#~ file_cpp := $(wildcard *.cpp)
|
||||
#~ file_cpp := ./test_turn.cpp
|
||||
#~ file_cpp := ./test_turn2.cpp
|
||||
#~ file_cpp := ./test_run.cpp
|
||||
file_cpp := ./test_psd.cpp
|
||||
|
||||
#~ file_o := /home/pi/usr/move.x
|
||||
#~ file_o := /home/pi/usr/turn.x
|
||||
file_o := /home/pi/usr/psd.x
|
||||
|
||||
$(file_o) : $(file_cpp)
|
||||
@$(cxx) $^ -o $@
|
||||
|
||||
maze : $(file_o)
|
||||
|
||||
clean :
|
||||
@rm -rf $(file_o)
|
||||
|
||||
debug :
|
||||
@echo $(cxx)
|
||||
@echo $(file_cpp)
|
||||
@echo $(file_o)
|
||||
|
||||
.PHONY : maze clean debug
|
||||
22
robot_test_code/实机测试/test_psd.cpp
Normal file
22
robot_test_code/实机测试/test_psd.cpp
Normal file
@@ -0,0 +1,22 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int Fwall,Lwall,Rwall,bwall;
|
||||
LCDMenu("","","","END");
|
||||
do
|
||||
{
|
||||
Fwall = PSDGetRaw(1)-2; //检测前面侧是否有墙
|
||||
Lwall = PSDGetRaw(2)-16; //检测左面
|
||||
Rwall = PSDGetRaw(3); //检测右面
|
||||
bwall = PSDGetRaw(4); //
|
||||
cout << "PSD L:" << Lwall << " R:" << Rwall << " F:" << Fwall << " b:" << bwall << endl; ;
|
||||
|
||||
} while (KEYRead() != KEY4);
|
||||
|
||||
}
|
||||
33
robot_test_code/实机测试/test_run.cpp
Normal file
33
robot_test_code/实机测试/test_run.cpp
Normal file
@@ -0,0 +1,33 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
LCDMenu("","","","END");
|
||||
//~ int num;
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,16);
|
||||
MOTORDriveRaw(2,14);
|
||||
|
||||
//~ cin>>num;
|
||||
//~ if(num)
|
||||
//~ {
|
||||
//~ MOTORDriveRaw(1,25);
|
||||
//~ MOTORDriveRaw(2,-24);
|
||||
//~ OSWait(1650);
|
||||
|
||||
//~ MOTORDriveRaw(1,18);
|
||||
//~ MOTORDriveRaw(2,-16);
|
||||
//~ OSWait(3500);
|
||||
|
||||
//~ MOTORDriveRaw(1,0);
|
||||
//~ MOTORDriveRaw(2,0);
|
||||
//~ }
|
||||
} while (KEYRead() != KEY4);
|
||||
|
||||
}
|
||||
132
robot_test_code/实机测试/test_turn.cpp
Normal file
132
robot_test_code/实机测试/test_turn.cpp
Normal file
@@ -0,0 +1,132 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include"eyebot++.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int f_wall, b_wall;
|
||||
int old_f_wall, old_b_wall;
|
||||
int turn_check;
|
||||
LCDMenu("","","","END");
|
||||
int num;
|
||||
do
|
||||
{
|
||||
cin>>num;
|
||||
if(num == 1)
|
||||
{
|
||||
b_wall = PSDGetRaw(4) - 2;
|
||||
do
|
||||
{
|
||||
old_b_wall = b_wall;
|
||||
MOTORDriveRaw(1,19);
|
||||
MOTORDriveRaw(2,-17);
|
||||
//~ MOTORDriveRaw(1,24);
|
||||
//~ MOTORDriveRaw(2,-22);
|
||||
OSWait(850);
|
||||
//~ OSWait(1980);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变
|
||||
//~ b_wall = PSDGetRaw(4) - 2;
|
||||
turn_check = b_wall - old_b_wall;
|
||||
/*---打印输出PSD数值---*/
|
||||
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
|
||||
cout << "b_wall:" << b_wall << " check:" << turn_check << endl;
|
||||
/*---打印结束---*/
|
||||
OSWait(50);
|
||||
} while ( turn_check < -3 || b_wall > 130);
|
||||
}
|
||||
else if (num == 3)
|
||||
{
|
||||
b_wall = PSDGetRaw(4) - 2;
|
||||
do
|
||||
{
|
||||
old_b_wall = b_wall;
|
||||
MOTORDriveRaw(1,-19);
|
||||
MOTORDriveRaw(2,17);
|
||||
//~ MOTORDriveRaw(1,24);
|
||||
//~ MOTORDriveRaw(2,-22);
|
||||
OSWait(850);
|
||||
//~ OSWait(1980);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变
|
||||
//~ b_wall = PSDGetRaw(4) - 2;
|
||||
turn_check = b_wall - old_b_wall;
|
||||
/*---打印输出PSD数值---*/
|
||||
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
|
||||
cout << "b_wall:" << b_wall << " check:" << turn_check << endl;
|
||||
/*---打印结束---*/
|
||||
OSWait(50);
|
||||
} while (turn_check < -3 || b_wall > 130);
|
||||
}
|
||||
else if (num == 2)
|
||||
{
|
||||
b_wall = PSDGetRaw(4) - 2;
|
||||
f_wall = PSDGetRaw(1) - 2;
|
||||
do
|
||||
{
|
||||
old_b_wall = b_wall;
|
||||
old_f_wall = f_wall;
|
||||
MOTORDriveRaw(1,19);
|
||||
MOTORDriveRaw(2,-17);
|
||||
//~ MOTORDriveRaw(1,24);
|
||||
//~ MOTORDriveRaw(2,-22);
|
||||
OSWait(1000);
|
||||
//~ OSWait(1980);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变
|
||||
f_wall = (PSDGetRaw(1) - 2) * 0.7 + old_f_wall * 0.3; //减小突变
|
||||
//~ b_wall = PSDGetRaw(4) - 2;
|
||||
//~ f_wall = PSDGetRaw(1) - 2;
|
||||
turn_check = b_wall - old_b_wall;
|
||||
/*---打印输出PSD数值---*/
|
||||
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
|
||||
cout << "f_wall: " << f_wall << "\t" << "b_wall: " << b_wall << "\t" << "check: " << turn_check << endl;
|
||||
/*---打印结束---*/
|
||||
OSWait(50);
|
||||
} while (f_wall < 200 || turn_check < -4 || b_wall > 130);
|
||||
}
|
||||
//~ if(num == 1)
|
||||
//~ {
|
||||
//~ MOTORDriveRaw(1,19);
|
||||
//~ MOTORDriveRaw(2,-17);
|
||||
//~ MOTORDriveRaw(1,24);
|
||||
//~ MOTORDriveRaw(2,-22);
|
||||
//~ OSWait(1500);
|
||||
//~ OSWait(1980);
|
||||
//~ MOTORDriveRaw(1,0);
|
||||
//~ MOTORDriveRaw(2,0);
|
||||
//~ }
|
||||
//~ else if(num == 2)
|
||||
//~ {
|
||||
//~ MOTORDriveRaw(1,24);
|
||||
//~ MOTORDriveRaw(2,-22);
|
||||
//~ MOTORDriveRaw(1,24);
|
||||
//~ MOTORDriveRaw(2,-22);
|
||||
//~ OSWait(2700);
|
||||
//~ OSWait(3250);
|
||||
//~ MOTORDriveRaw(1,0);
|
||||
//~ MOTORDriveRaw(2,0);
|
||||
//~ }
|
||||
//~ else if(num == 3)
|
||||
//~ {
|
||||
//~ MOTORDriveRaw(1,26);
|
||||
//~ MOTORDriveRaw(2,-24);
|
||||
//~ MOTORDriveRaw(1,-24);
|
||||
//~ MOTORDriveRaw(2,22);
|
||||
//~ OSWait(1815);
|
||||
//~ OSWait(2025);
|
||||
//~ MOTORDriveRaw(1,0);
|
||||
//~ MOTORDriveRaw(2,0);
|
||||
//~ }
|
||||
else
|
||||
{
|
||||
}
|
||||
} while (KEYRead() != KEY4);
|
||||
|
||||
}
|
||||
79
robot_test_code/实机测试/test_turn2.cpp
Normal file
79
robot_test_code/实机测试/test_turn2.cpp
Normal file
@@ -0,0 +1,79 @@
|
||||
#include<iostream>
|
||||
#include<string>
|
||||
#include<cmath>
|
||||
//~ #include"eyebot++.h"
|
||||
#include"/home/pi/eyebot/include/eyebot++.h"
|
||||
using namespace std;
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
LCDMenu("","","","END");
|
||||
int num;
|
||||
//~ int pwm1, pwm2;
|
||||
do
|
||||
{
|
||||
cin>>num;
|
||||
if (num == 1)
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,21);
|
||||
MOTORDriveRaw(2,-19);
|
||||
} while (abs(ENCODERRead(2)) < 217);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
|
||||
else if (num == 2)
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,21);
|
||||
MOTORDriveRaw(2,-19);
|
||||
} while (abs(ENCODERRead(2)) < 500);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
|
||||
else if (num == 3)
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,-21);
|
||||
MOTORDriveRaw(2,19);
|
||||
} while (abs(ENCODERRead(2)) < 242);
|
||||
MOTORDriveRaw(1,0);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
|
||||
else if (num == 4)
|
||||
{
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(1,-21);
|
||||
} while (abs(ENCODERRead(1)) < 285);
|
||||
MOTORDriveRaw(1,0);
|
||||
do
|
||||
{
|
||||
MOTORDriveRaw(2,19);
|
||||
} while (abs(ENCODERRead(2)) < 285);
|
||||
MOTORDriveRaw(2,0);
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
cout << " num error " << endl;
|
||||
}
|
||||
|
||||
} while (KEYRead() != KEY4);
|
||||
|
||||
}
|
||||
10
文件/eyebot函数.cpp
Normal file
10
文件/eyebot函数.cpp
Normal file
@@ -0,0 +1,10 @@
|
||||
|
||||
OSWait();
|
||||
PSDGetRaw();
|
||||
MOTORDriveRaw();
|
||||
VWSetSpeed();
|
||||
VWGetPosition();
|
||||
VWSetPosition();
|
||||
VWWait();
|
||||
ENCODERReset();
|
||||
ENCODERRead();
|
||||
97
文件/hdt.txt
Normal file
97
文件/hdt.txt
Normal file
@@ -0,0 +1,97 @@
|
||||
# EYEBOT Name
|
||||
EYEBOT IBot-D
|
||||
|
||||
# MOTOR Number | TableName
|
||||
MOTOR 1 Motor_Table
|
||||
MOTOR 2 Motor_Table
|
||||
|
||||
# Servo Number | Low | High |TableName
|
||||
SERVO 1 0 255 Servo_Table
|
||||
SERVO 2 0 255 Servo_Table
|
||||
SERVO 3 0 255 Servo_Table
|
||||
SERVO 4 0 255 Servo_Table
|
||||
|
||||
# ENCODER Number | Clicks per meter
|
||||
ENCODER 1 6900
|
||||
ENCODER 2 6900
|
||||
|
||||
# PSD NUMER | TableName
|
||||
PSD 1 PSD_TableA
|
||||
PSD 2 PSD_TableA
|
||||
PSD 3 PSD_TableA
|
||||
PSD 4 PSD_TableA
|
||||
|
||||
# WIFI Default_Hotspot(0)/Slave(1)/Custom_Hotspot(2)
|
||||
WIFI 0
|
||||
|
||||
# HOTSPOT Network_Name | Password
|
||||
# HOTSPOT rob rasp
|
||||
|
||||
# SLAVE Network_Name | Password
|
||||
SLAVE SSID PASS
|
||||
|
||||
# RoBIOS On(1)/Off(0) | FontSize
|
||||
ROBIOS 1 10
|
||||
|
||||
# USB Number | DeviceName
|
||||
USB 1 EyeBot
|
||||
USB 2 GPS
|
||||
|
||||
# DISPLAY LCD(0)/HDMI(1) | Fullscreen Off/On (0/1) | Rotation (0/1) | Autorefresh (0/1)
|
||||
# Note: Change of rotation requires 2 reboots
|
||||
LCD 0 0 0 1
|
||||
|
||||
# RPI RaspPi_Ver(1/2/3/4)
|
||||
RPI 4
|
||||
|
||||
# DRIVE Wheel distance (90 or 140mm) | Max Motor Speed | Motor1/left dir. (0 = c/w) | Motor2/right dir. (1=clockwise)
|
||||
DRIVE 110 200 1 2
|
||||
|
||||
#DEMOPATH | Path
|
||||
DEMOPATH /home/pi/eyebot/demo
|
||||
|
||||
#SOFTWAREPATH | Path
|
||||
SOFTWAREPATH /home/pi/usr
|
||||
|
||||
# VOMEGA | Kp | Ki | Kd | Klink (Note if 0,0,0,0 will turn off)
|
||||
# ... PID frequency, Dead-Rockoning min. change for update, Debug-Flag
|
||||
VOMEGA 400 40 10 50 10 20 0
|
||||
|
||||
# -------------------- TABLES (Optional) ------------------
|
||||
# Motor Linearisation Table 101 values: 0 .. 100
|
||||
TABLE Motor_Table
|
||||
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
|
||||
20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
|
||||
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
|
||||
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
|
||||
80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
|
||||
100
|
||||
END TABLE
|
||||
|
||||
# Servo Linearisation Table 256 values: 0 .. 255
|
||||
TABLE Servo_Table
|
||||
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
|
||||
20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
|
||||
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
|
||||
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
|
||||
80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
|
||||
100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119
|
||||
120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
|
||||
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159
|
||||
160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
|
||||
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199
|
||||
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219
|
||||
220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239
|
||||
240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255
|
||||
END TABLE
|
||||
|
||||
# PSD Sensor Linearisation Table 128 values in [mm]:
|
||||
TABLE PSD_TableA
|
||||
800 800 800 800 800 800 800 800
|
||||
800 800 800 800 800 800 800 800 800 800 800 770 750 720 700 680 650 610 590 560
|
||||
540 530 480 470 460 450 440 430 420 410 400 390 380 370 360 350 340 330 330 320
|
||||
300 290 280 280 280 270 270 260 250 250 250 240 240 230 230 220 220 210 210 210
|
||||
200 200 190 190 190 190 180 180 170 170 170 160 160 160 160 150 150 150 150 150
|
||||
140 140 140 140 140 130 130 130 130 130 130 120 120 120 120 110 110 110 110 110
|
||||
110 110 110 110 110 110 110 110 110 100 100 100 100 100 100 100 90 90 90 90
|
||||
END TABLE
|
||||
BIN
文件/maze2.x
Executable file
BIN
文件/maze2.x
Executable file
Binary file not shown.
147
文件/新建 文本文档.txt
Normal file
147
文件/新建 文本文档.txt
Normal file
@@ -0,0 +1,147 @@
|
||||
#include "eyebot.h"
|
||||
#define SAFE 200
|
||||
|
||||
static int v_des1,v_des2;
|
||||
|
||||
float Kp=0.25;
|
||||
float Ki=0.06;
|
||||
float Klink=0.05;
|
||||
|
||||
TIMER t1;
|
||||
|
||||
void picontroller()
|
||||
{
|
||||
int enc_new1,v_act1,r_mot1,e_func1;
|
||||
int enc_new2,v_act2,r_mot2,e_func2;
|
||||
static int enc_old1,enc_old2;
|
||||
static int r_old1=0,e_old1=0;
|
||||
static int r_old2=0,e_old2=0;
|
||||
|
||||
enc_new1=ENCODERRead(1);
|
||||
enc_new2=ENCODERRead(2);
|
||||
|
||||
v_act1=enc_new1-enc_old1;
|
||||
v_act2=enc_new2-enc_old2;
|
||||
|
||||
e_func1=v_des1-v_act1;
|
||||
e_func2=v_des2-v_act2;
|
||||
|
||||
|
||||
r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2));
|
||||
r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2));
|
||||
|
||||
if(v_des1>0&&v_des2>0)
|
||||
{
|
||||
if(r_mot1>35)r_mot1=35;
|
||||
else if(r_mot1<0) r_mot1=0;
|
||||
|
||||
if(r_mot2>35)r_mot2=35;
|
||||
else if(r_mot2<0) r_mot2=0;
|
||||
}
|
||||
else if(v_des1>0&&v_des2<0)
|
||||
{
|
||||
if(r_mot1>35)r_mot1=35;
|
||||
else if(r_mot1<0) r_mot1=0;
|
||||
if(r_mot2>0)r_mot2=0;
|
||||
else if(r_mot2<-35) r_mot2=-35;
|
||||
}
|
||||
else if(v_des1<0&&v_des2>0)
|
||||
{
|
||||
if(r_mot1>0)r_mot1=0;
|
||||
else if(r_mot1<-35) r_mot1=-35;
|
||||
if(r_mot2>35)r_mot2=35;
|
||||
else if(r_mot2<0) r_mot2=0;
|
||||
}
|
||||
else if(v_des1==0&&v_des2==0)
|
||||
{
|
||||
r_mot1=0;
|
||||
r_mot2=0;
|
||||
v_act1=0;
|
||||
v_act2=0;
|
||||
enc_old1=0;
|
||||
r_old1=0;
|
||||
enc_old2=0;
|
||||
r_old2=0;
|
||||
e_func1=0;
|
||||
e_func2=0;
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
}
|
||||
|
||||
MOTORDrive(1,r_mot1);
|
||||
MOTORDrive(2,r_mot2);
|
||||
|
||||
enc_old1=enc_new1;
|
||||
r_old1=r_mot1;
|
||||
e_old1=e_func1;
|
||||
|
||||
enc_old2=enc_new2;
|
||||
r_old2=r_mot2;
|
||||
e_old2=e_func2;
|
||||
|
||||
printf("%d %d %d %d\n",r_mot1,r_mot2,enc_old1,enc_old2);
|
||||
|
||||
}
|
||||
|
||||
void VWSpeed(int v1,int v2)
|
||||
{
|
||||
v_des1=v1;
|
||||
v_des2=v2;
|
||||
}
|
||||
|
||||
|
||||
void drive()
|
||||
{
|
||||
do
|
||||
{
|
||||
if(PSDGetRaw(PSD_LEFT)<SAFE)
|
||||
VWSpeed(100,90);
|
||||
else VWSpeed(90,100);
|
||||
|
||||
OSWait(80);
|
||||
|
||||
}while(PSDGetRaw(PSD_FRONT)>SAFE);
|
||||
VWSpeed(0,0);
|
||||
OSWait(1000);
|
||||
}
|
||||
|
||||
void turn()
|
||||
{
|
||||
VWSpeed(50,-50);
|
||||
while(PSDGetRaw(PSD_FRONT)<400) OSWait(80);
|
||||
VWSpeed(0,0);
|
||||
OSWait(1000);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
LCDMenu("STOP","2","3","END");
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
|
||||
PSDGetRaw(PSD_FRONT);
|
||||
PSDGetRaw(PSD_LEFT);
|
||||
|
||||
|
||||
t1=OSAttachTimer(100,picontroller);
|
||||
|
||||
while(KEYRead()!=KEY1)
|
||||
{ drive();
|
||||
turn();
|
||||
}
|
||||
|
||||
OSDetachTimer(t1);
|
||||
|
||||
AUBeep();
|
||||
|
||||
while(KEYRead()!=KEY4)
|
||||
{
|
||||
MOTORDrive(1,0);
|
||||
MOTORDrive(2,0);
|
||||
}
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
221
文件/终端常用.ini
Normal file
221
文件/终端常用.ini
Normal file
@@ -0,0 +1,221 @@
|
||||
|
||||
[eyesim]
|
||||
#切换到project路径下:
|
||||
cd /cygdrive/d/桌面/sync同步文件夹/sync同步文件夹/本科毕设/bk_bishe/eyesim/project
|
||||
cd /cygdrive/d/桌面/sync同步文件夹/sync同步文件夹/本科毕设/bk_bishe/eyesim/all_test01
|
||||
cd /cygdrive/d/桌面/sync同步文件夹/sync同步文件夹/本科毕设/bk_bishe/编写代码
|
||||
cd /cygdrive/d/桌面/sync同步文件夹/sync同步文件夹/本科毕设/bk_bishe/C++功能测试/arr_ test/
|
||||
|
||||
|
||||
|
||||
#编译PID:
|
||||
g++sim PIDmain.cpp PID.cpp -o maze.x
|
||||
#编译递归探索部分:
|
||||
g++sim maze_main.cpp maze_explore.cpp -o maze.x
|
||||
|
||||
[树莓派]
|
||||
#关机：
|
||||
sudo halt
|
||||
|
||||
#编译：
|
||||
gccarm myfile.cpp -o myfile.x
|
||||
|
||||
|
||||
[git]
|
||||
#查看：
|
||||
git gelper#查看所有的git命令
|
||||
|
||||
##---从远程仓库克隆/获取---##
|
||||
git clone <repo> #从远程仓库克隆代码
|
||||
git clone -b 分支名 地址 #克隆分支代码到本地
|
||||
git clone <repo> <directory>#将代码内容克隆到指定目录
|
||||
参数：
|
||||
repo :仓库地址
|
||||
directory:本地目录
|
||||
|
||||
例如：
|
||||
git clone git://github.com/schacon/grit.git
|
||||
git clone git://github.com/schacon/grit.git mygrit # 克隆到指定本地目录
|
||||
# 不同协议的仓库地址
|
||||
git clone http://github.com/CosmosHua/locate new --HTTP协议
|
||||
git clone git://github.com/CosmosHua/locate new --GIT协议
|
||||
git clone https://github.com/fsliurujie/test.git --HTTPS协议
|
||||
|
||||
|
||||
##---添加文件到暂存区（追踪文件）---##
|
||||
# 添加指定文件到暂存区
|
||||
git add 文件名
|
||||
|
||||
# 提交多个文件到暂存区
|
||||
git add 文件1,文件2
|
||||
|
||||
# 将所有文件提交到暂存区
|
||||
git add .
|
||||
|
||||
# 一个文件分多次提交
|
||||
git add -p 文件名
|
||||
|
||||
##---提交文件到本地仓库---##
|
||||
# 全部提交到本地仓库
|
||||
git commit
|
||||
|
||||
# 提交指定文件到本地仓库
|
||||
git commit
|
||||
|
||||
# 添加提交备注信息，在提交文件后立马用
|
||||
git commit -m "提交的备注信息"
|
||||
|
||||
# 将工作区修改或删除的文件提交到本地版本库，新增的文件不会被提交
|
||||
git commit -am "备注信息"
|
||||
|
||||
# 修改最新一条提交记录的提交原因
|
||||
git commit --amend - m "提交原因"
|
||||
|
||||
# 将当前文件改动提交到HEAD或当前分支的历史ID
|
||||
git commit -C HEAD
|
||||
注： 在 Linux 系统中,'commit 信息使用单引号 ',"Windows 系统,commit 信息使用双引号 "。
|
||||
|
||||
##---远程操作---##
|
||||
git remote -v # 查看远程版本库信息
|
||||
|
||||
git branch -r # 查看远程分支信息
|
||||
|
||||
git remote show <remote> # 查看远程版本库信息
|
||||
|
||||
git remote add <remote> <url> # 添加远程版本库，<remote>是给远程仓库命名，通常是origin
|
||||
|
||||
git fetch <remote> # 从远程代码库获取最新代码（不合并到本地分支）
|
||||
|
||||
git pull <remote> <branch> # 获取最新代码并快速合并
|
||||
|
||||
git push <remote> <branch> # 上传代码并快速合并，<branch>是指将代码上传到该分支上
|
||||
|
||||
git push <remote> :<branch/tag-name> # 删除远程分支或标签
|
||||
git push origin --delete <分支名称/标签名称> # 删除远程分支或标签
|
||||
|
||||
git push -tags # 上传所有标签
|
||||
|
||||
##---查看修改历史---##
|
||||
# 查看指定文件的修改历史
|
||||
git blame 文件名
|
||||
|
||||
# 从100行开始，到110行 逐行查看文件的修改历史
|
||||
git blame -L 100,10 文件名
|
||||
|
||||
|
||||
##打开图形化工具##
|
||||
gitk
|
||||
|
||||
##删除文件##
|
||||
# 将文件从暂存区和工作区删除
|
||||
git rm 文件名
|
||||
|
||||
# 删除之前修改过并且已经放到暂存区域的话，则必须要用强制删除选项 -f
|
||||
git rm -f 文件名
|
||||
|
||||
# 只删除暂存区的文件，工作区保留
|
||||
git rm --cached 文件名
|
||||
|
||||
# 清除缓存区
|
||||
git rm -r --cached .
|
||||
|
||||
|
||||
##---git回退操作---##
|
||||
git reset [--soft | --mixed | --hard] [HEAD]
|
||||
# --soft 指针移动的时候，工作区内容不变，重置git commit提交
|
||||
【简单来说就是取消commit操作，提交过的文件恢复到暂存区】
|
||||
|
||||
# --mixed 为默认，可以省略该参数，指针移动，重置git add、git commit操作
|
||||
【简单来说就是取消add和commit操作，将提交的内容恢复到暂存区，再将暂存区的内容恢复到工作区】
|
||||
|
||||
# --hard 指针移动，工作区、暂存区内容删除，回到上一个提交的版本
|
||||
【简单来说就是HEAD指针移动到指定版本后之后，其后面版本的工作区、、缓存区内容全部清除】
|
||||
|
||||
# 回退一个版本，将已提交的内容恢复到暂存区，不影响工作区的文件(未提交的也不受影响)
|
||||
git reset HEAD^ --soft
|
||||
|
||||
# 回退一个版本，重置暂存区的文件与上一次的提交(commit)保持一致，不影响原来工作区(未提交的也不受影响)
|
||||
git reset HEAD^
|
||||
# 或者写成
|
||||
git reset HEAD^ --mixed
|
||||
|
||||
# 回退一个版本，清空暂存区，将上个版本的内容完全替代本地工作区版本内容
|
||||
git reset HEAD^ --hard
|
||||
|
||||
# 回退到该次提交id的位置 并将回撤内容保存在暂存区
|
||||
git reset --soft commitID
|
||||
|
||||
# 将本地的状态回退到和远程仓库的一样
|
||||
git reset --hard origin/master
|
||||
|
||||
# 从仓库回退到暂存区
|
||||
git reset --files
|
||||
|
||||
# 所有内容都回撤完了后将回撤后的内容强制推送到远程分支
|
||||
git push -f -u origin 分支名
|
||||
|
||||
##---回退n个版本---##
|
||||
git reset HEAD # 表示当前版本
|
||||
git reset HEAD^ # 回退1个版本
|
||||
git reset HEAD^^ # 回退2个版本
|
||||
git reset HEAD^^^ # 回退3个版本
|
||||
...
|
||||
|
||||
# 回退n个版本
|
||||
git reset HEAD~n
|
||||
# 或者
|
||||
git reset HEAD^n
|
||||
|
||||
|
||||
##---分支---##
|
||||
# 新建分支
|
||||
git branch 分支名
|
||||
|
||||
# 查看当前所有本地分支
|
||||
git branch
|
||||
|
||||
# 切换分支
|
||||
git checkout 分支名
|
||||
|
||||
# 创建并切换分支
|
||||
git checkout -b 分支名
|
||||
|
||||
# 还原这个文件到对应的commitId的版本
|
||||
git checkout commitId 文件名（文件路径下的文件名）
|
||||
|
||||
# 查看本地当前分支以及提交hash值和commit信息
|
||||
git branch -v
|
||||
|
||||
# 查看全部分支(列出远程分支以及本地分支名 远程分支会以remote/origin/分支名这种形式展示 红色标识)
|
||||
git branch -a
|
||||
|
||||
# 查看带有最后提交id、最近提交原因等信息的本地版本库分支列表
|
||||
git branch -vv
|
||||
|
||||
# 列出远程分支(远程所有分支名)
|
||||
git branch -r
|
||||
|
||||
# 合并指定分支到当前分支
|
||||
git merge 指定分支名
|
||||
|
||||
# 把其他分支的某一次提交内容合并到当前分支
|
||||
git cherry-pick commitId
|
||||
|
||||
# 删除本地分支
|
||||
git branch -d 分支名
|
||||
|
||||
# 分支未提交到本地版本库前强制删除分支
|
||||
git branch -D 分支名
|
||||
|
||||
# 修改分支名
|
||||
git branch -m 旧分支名 新分支名
|
||||
|
||||
# 修改分支名 M强制修改 若与其他分支有冲突也会创建(慎用)
|
||||
git branch -M 旧分支名 新分支名
|
||||
|
||||
# 删除远程版本库上的分支
|
||||
git push origin --delete <分支名称>
|
||||
git push origin :<分支名称>
|
||||
|
||||
# 将本地分支与远程分支相关联
|
||||
git push -u origin <本地分支名称>
|
||||
147
文件/老师的函数.txt
Normal file
147
文件/老师的函数.txt
Normal file
@@ -0,0 +1,147 @@
|
||||
#include "eyebot.h"
|
||||
#define SAFE 200
|
||||
|
||||
static int v_des1,v_des2;
|
||||
|
||||
float Kp=0.25;
|
||||
float Ki=0.06;
|
||||
float Klink=0.05;
|
||||
|
||||
TIMER t1;
|
||||
|
||||
void picontroller()
|
||||
{
|
||||
int enc_new1,v_act1,r_mot1,e_func1;
|
||||
int enc_new2,v_act2,r_mot2,e_func2;
|
||||
static int enc_old1,enc_old2;
|
||||
static int r_old1=0,e_old1=0;
|
||||
static int r_old2=0,e_old2=0;
|
||||
|
||||
enc_new1=ENCODERRead(1);
|
||||
enc_new2=ENCODERRead(2);
|
||||
|
||||
v_act1=enc_new1-enc_old1;
|
||||
v_act2=enc_new2-enc_old2;
|
||||
|
||||
e_func1=v_des1-v_act1;
|
||||
e_func2=v_des2-v_act2;
|
||||
|
||||
|
||||
r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2));
|
||||
r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2));
|
||||
|
||||
if(v_des1>0&&v_des2>0)
|
||||
{
|
||||
if(r_mot1>35)r_mot1=35;
|
||||
else if(r_mot1<0) r_mot1=0;
|
||||
|
||||
if(r_mot2>35)r_mot2=35;
|
||||
else if(r_mot2<0) r_mot2=0;
|
||||
}
|
||||
else if(v_des1>0&&v_des2<0)
|
||||
{
|
||||
if(r_mot1>35)r_mot1=35;
|
||||
else if(r_mot1<0) r_mot1=0;
|
||||
if(r_mot2>0)r_mot2=0;
|
||||
else if(r_mot2<-35) r_mot2=-35;
|
||||
}
|
||||
else if(v_des1<0&&v_des2>0)
|
||||
{
|
||||
if(r_mot1>0)r_mot1=0;
|
||||
else if(r_mot1<-35) r_mot1=-35;
|
||||
if(r_mot2>35)r_mot2=35;
|
||||
else if(r_mot2<0) r_mot2=0;
|
||||
}
|
||||
else if(v_des1==0&&v_des2==0)
|
||||
{
|
||||
r_mot1=0;
|
||||
r_mot2=0;
|
||||
v_act1=0;
|
||||
v_act2=0;
|
||||
enc_old1=0;
|
||||
r_old1=0;
|
||||
enc_old2=0;
|
||||
r_old2=0;
|
||||
e_func1=0;
|
||||
e_func2=0;
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
}
|
||||
|
||||
MOTORDrive(1,r_mot1);
|
||||
MOTORDrive(2,r_mot2);
|
||||
|
||||
enc_old1=enc_new1;
|
||||
r_old1=r_mot1;
|
||||
e_old1=e_func1;
|
||||
|
||||
enc_old2=enc_new2;
|
||||
r_old2=r_mot2;
|
||||
e_old2=e_func2;
|
||||
|
||||
printf("%d %d %d %d\n",r_mot1,r_mot2,enc_old1,enc_old2);
|
||||
|
||||
}
|
||||
|
||||
void VWSpeed(int v1,int v2)
|
||||
{
|
||||
v_des1=v1;
|
||||
v_des2=v2;
|
||||
}
|
||||
|
||||
|
||||
void drive()
|
||||
{
|
||||
do
|
||||
{
|
||||
if(PSDGetRaw(PSD_LEFT)<SAFE)
|
||||
VWSpeed(100,90);
|
||||
else VWSpeed(90,100);
|
||||
|
||||
OSWait(80);
|
||||
|
||||
}while(PSDGetRaw(PSD_FRONT)>SAFE);
|
||||
VWSpeed(0,0);
|
||||
OSWait(1000);
|
||||
}
|
||||
|
||||
void turn()
|
||||
{
|
||||
VWSpeed(50,-50);
|
||||
while(PSDGetRaw(PSD_FRONT)<400) OSWait(80);
|
||||
VWSpeed(0,0);
|
||||
OSWait(1000);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
LCDMenu("STOP","2","3","END");
|
||||
ENCODERReset(1);
|
||||
ENCODERReset(2);
|
||||
|
||||
PSDGetRaw(PSD_FRONT);
|
||||
PSDGetRaw(PSD_LEFT);
|
||||
|
||||
|
||||
t1=OSAttachTimer(100,picontroller);
|
||||
|
||||
while(KEYRead()!=KEY1)
|
||||
{ drive();
|
||||
turn();
|
||||
}
|
||||
|
||||
OSDetachTimer(t1);
|
||||
|
||||
AUBeep();
|
||||
|
||||
while(KEYRead()!=KEY4)
|
||||
{
|
||||
MOTORDrive(1,0);
|
||||
MOTORDrive(2,0);
|
||||
}
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user