yyz/bk_bishe_pi
1
0
Fork 0
mirror of https://gitee.com/yyz_o/bk_bishe_pi.git synced 2025-09-07 23:21:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

2025.5.18晚上提交，实机调试后的代码，有一次成功了，但是后面没复现了，很奇怪

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-18 22:47:24 +08:00
parent 1155ae6f6d
commit 00e0d9366d
13 changed files with 129 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
bot_code/maze_func_explore.cpp
View File

@@ -104,14 +104,16 @@ void maze_entry(int x, int y, int dir, int open)
{ {
if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空
{ {
// std::vector<int> mark_insert(mark[0].size(), 0); //~ std::vector<int> mark_insert(mark[0].size(), 0);
// mark.insert(mark.begin(), mark_insert); //~ mark.insert(mark.begin(), mark_insert);
// std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0)); //~ std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
// wall.insert(wall.begin(), wall_insert); //~ wall.insert(wall.begin(), wall_insert);
mark.emplace(mark.begin(), std::vector<int>(mark[0].size(), 0)); //在mark头部插入一个内层 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头部插入一个内层 wall.emplace(wall.begin(), std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0))); //在wall头部插入一个内层
rob_x++;
rob_x0++;
} }
else else
{ {
@@ -134,6 +136,8 @@ void maze_entry(int x, int y, int dir, int open)
{ {
wall[i].emplace(wall[i].begin(), std::vector<int>(2,0)); wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
} }
rob_y++;
rob_y0++;
} }
else else
{ {

6
bot_code/maze_func_goto.cpp
View File

@@ -75,13 +75,13 @@ void go_to(int dir)
if (turn) //如果turn不是0，则执行旋转命令 if (turn) //如果turn不是0，则执行旋转命令
{ {
OSWait(1000); OSWait(2000);
BOTturn(turn); BOTturn(turn);
OSWait(200); OSWait(2000);
VWWait(); VWWait();
} }
PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格 PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格
//~ OSWait(200); //~ OSWait(1000);
VWWait(); //等待下一条指令 VWWait(); //等待下一条指令
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向 rob_dir = maze_dir; //更新机器人移动到下一格之后的方向

36
bot_code/maze_func_pid.cpp
View File

@@ -42,8 +42,8 @@ void pid_speed_xianfu()
/*---位置式PID算法---*/ /*---位置式PID算法---*/
void PID_AL() void PID_AL()
{ {
//~ err_sum += err; //积分累计误差值 err_sum += err; //积分累计误差值
//~ eI_xianfu(); //积分部分限幅 eI_xianfu(); //积分部分限幅
err_diff = err - err_old; //微分部分误差值 err_diff = err - err_old; //微分部分误差值
/*---打印输出PID控制的各项参数1---*/ /*---打印输出PID控制的各项参数1---*/
//~ cout << "err:" << err << " " ; //~ cout << "err:" << err << " " ;
@@ -54,19 +54,21 @@ void PID_AL()
err_old = err; //将误差幅值到上一次误差 err_old = err; //将误差幅值到上一次误差
//~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 //~ 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 + Ki*err_sum + Kd*err_diff) * Kpid_base;
Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base; //~ Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
pid_speed_xianfu(); pid_speed_xianfu();
speed_r = speed_l + Kpid_speed; //~ speed_r = speed_l + Kpid_speed;
/*---打印输出PID控制的各项参数2---*/ /*---打印输出PID控制的各项参数2---*/
cout << "PID_out:" << Kpid_speed << endl ; //~ cout << "PID_out:" << Kpid_speed << endl ;
//~ cout << "PID_out:" << Kpid_speed << " " ;
/*---打印结束---*/ /*---打印结束---*/
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
MOTORDriveRaw(2,speed_r); MOTORDriveRaw(2,speed_l - 2 + Kpid_speed);
//~ MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l); MOTORDriveRaw(1,speed_l);
} }
@@ -77,16 +79,26 @@ void PIDStraight()
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值 int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
//~ cout << x_1 << " " << y_1 << " " << phi_1 << endl;
MOTORDriveRaw(2,speed_r); 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); MOTORDriveRaw(1,speed_l);
do do
{ {
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 //~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 //~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 //~ 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数值---*/ /*---打印输出PSD数值---*/
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
@@ -116,7 +128,7 @@ void PIDStraight()
} }
VWGetPosition(&x_2, &y_2, &phi_2); VWGetPosition(&x_2, &y_2, &phi_2);
bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2)); bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
//~ cout << "move:" << bot_move << endl; cout << "move:" << bot_move << endl;
}while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 }while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行

17
bot_code/maze_func_turn.cpp
View File

@@ -14,10 +14,10 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,21); MOTORDriveRaw(1,20);
MOTORDriveRaw(2,-19); MOTORDriveRaw(2,-18);
} while (abs(ENCODERRead(2)) < 218); } while (abs(ENCODERRead(2)) < 240);
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
@@ -28,8 +28,8 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,-21); MOTORDriveRaw(1,-20);
MOTORDriveRaw(2,19); MOTORDriveRaw(2,18);
} while (abs(ENCODERRead(2)) < 240); } while (abs(ENCODERRead(2)) < 240);
@@ -42,12 +42,13 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,21); MOTORDriveRaw(1,20);
MOTORDriveRaw(2,-19); MOTORDriveRaw(2,-18);
} while (abs(ENCODERRead(2)) < 500); } while (abs(ENCODERRead(2)) < 550);
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
//~ speed_r = speed_l - Kpid_speed;
} }
else else
{ {

7
bot_code/maze_parameter.cpp
View File

@@ -9,9 +9,9 @@ using namespace std;
/*---定义数据---*/ /*---定义数据---*/
/*---定义全局常量---*/ /*---定义全局常量---*/
const int DIST_cell = 240; //单元格长度 const int DIST_cell = 240; //单元格长度
const int DIST_move = 292; //移动的单元格距离 const int DIST_move = 330; //移动的单元格距离
const int DIST_wall_F = 90; //与墙壁的距离 const int DIST_wall_F = 90; //与墙壁的距离
const int DIST_wall_RL = 88; //与左右墙壁的距离 const int DIST_wall_RL = 90; //与左右墙壁的距离
/*---定义全局变量---*/ /*---定义全局变量---*/
/*---容器类---*/ /*---容器类---*/
@@ -29,9 +29,10 @@ int rob_dir = 0; //机器人当前的朝向（初始为0）
/*---PID算法---*/ /*---PID算法---*/
const float Kp = 0.4, Ki = 0, Kd = 0.25; //定义PID的参数17 //~ 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.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 = 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计算后输出的倍率 //~ float Kpid; //定义PID计算后输出的倍率
int Kpid_speed; int Kpid_speed;
const float Kpid_base = 1; //定义基本角速度和速度 const float Kpid_base = 1; //定义基本角速度和速度

2
bot_code_turn/makefile
View File

@@ -2,7 +2,7 @@
cxx := gccarm cxx := gccarm
file_cpp := $(wildcard *.cpp) file_cpp := $(wildcard *.cpp)
file_o := /home/pi/usr/maze.x file_o := /home/pi/usr/maze2.x
$(file_o) : $(file_cpp) $(file_o) : $(file_cpp)
@$(cxx) $^ -o $@ @$(cxx) $^ -o $@

4
bot_code_turn/maze_func_explore.cpp
View File

@@ -112,6 +112,8 @@ void maze_entry(int x, int y, int dir, int open)
mark.emplace(mark.begin(), std::vector<int>(mark[0].size(), 0)); //在mark头部插入一个内层 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头部插入一个内层 wall.emplace(wall.begin(), std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0))); //在wall头部插入一个内层
rob_x++;
rob_x0++;
} }
else else
{ {
@@ -134,6 +136,8 @@ void maze_entry(int x, int y, int dir, int open)
{ {
wall[i].emplace(wall[i].begin(), std::vector<int>(2,0)); wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
} }
rob_y++;
rob_y0++;
} }
else else
{ {

8
bot_code_turn/maze_func_goto.cpp
View File

@@ -75,14 +75,16 @@ void go_to(int dir)
if (turn) //如果turn不是0，则执行旋转命令 if (turn) //如果turn不是0，则执行旋转命令
{ {
OSWait(1000); OSWait(2000);
BOTturn(turn); BOTturn(turn);
OSWait(200); OSWait(2000);
VWWait(); VWWait();
} }
PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格 PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格
//~ OSWait(200); //~ OSWait(200);
VWWait(); //等待下一条指令 //~ VWWait(); //等待下一条指令
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向 rob_dir = maze_dir; //更新机器人移动到下一格之后的方向
xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标 xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标

2
bot_code_turn/maze_func_output.cpp
View File

@@ -13,7 +13,7 @@ void output_arr2D(int size_x, int size_y, int *arr)
{ {
for (int j = 0; j < size_x; j++) for (int j = 0; j < size_x; j++)
{ {
cout << arr[(j * size_y) + i] << " "; cout << arr[(j * size_y) + i] << "\t";
} }
cout << endl; cout << endl;
} }

59
bot_code_turn/maze_func_pid.cpp
View File

@@ -1,6 +1,7 @@
#include<iostream> #include<iostream>
#include<string> #include<string>
#include"eyebot++.h" #include"eyebot++.h"
#include<cmath>
#include"maze_parameter.h" #include"maze_parameter.h"
#include"maze_func.h" #include"maze_func.h"
using namespace std; using namespace std;
@@ -42,8 +43,8 @@ void pid_speed_xianfu()
/*---位置式PID算法---*/ /*---位置式PID算法---*/
void PID_AL() void PID_AL()
{ {
//~ err_sum += err; //积分累计误差值 err_sum += err; //积分累计误差值
//~ eI_xianfu(); //积分部分限幅 eI_xianfu(); //积分部分限幅
err_diff = err - err_old; //微分部分误差值 err_diff = err - err_old; //微分部分误差值
/*---打印输出PID控制的各项参数1---*/ /*---打印输出PID控制的各项参数1---*/
//~ cout << "err:" << err << " " ; //~ cout << "err:" << err << " " ;
@@ -54,20 +55,26 @@ void PID_AL()
err_old = err; //将误差幅值到上一次误差 err_old = err; //将误差幅值到上一次误差
//~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 //~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
//~ Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base; //~ if (abs(err_sum) > 100 )
Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base; //~ {
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(); pid_speed_xianfu();
speed_r = speed_l + Kpid_speed; //~ speed_r = speed_l - 1+ Kpid_speed;
/*---打印输出PID控制的各项参数2---*/ /*---打印输出PID控制的各项参数2---*/
cout << "PID_out:" << Kpid_speed << endl ; //~ cout << "PID_out:" << Kpid_speed << endl ;
/*---打印结束---*/ /*---打印结束---*/
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
MOTORDriveRaw(2,speed_r); MOTORDriveRaw(2,speed_r + Kpid_speed);
MOTORDriveRaw(1,speed_l); //~ MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l - Kpid_speed);
} }
/*---PID控制下的直线行驶---*/ /*---PID控制下的直线行驶---*/
@@ -78,48 +85,54 @@ void PIDStraight()
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 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(2,speed_r);
MOTORDriveRaw(1,speed_l); MOTORDriveRaw(1,speed_l);
do do
{ {
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 //~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 //~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 //~ 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数值---*/ /*---打印输出PSD数值---*/
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; //~ 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) //如果两侧都有墙的情况 if (20<L_PSD && L_PSD<125 && 20<R_PSD && R_PSD<125) //如果两侧都有墙的情况
{ {
err = L_PSD - R_PSD; err = L_PSD - R_PSD;
PID_AL(); PID_AL();
} }
else if (20<L_PSD && L_PSD<140) //只有左侧有墙的情况 else if (20<L_PSD && L_PSD<150) //只有左侧有墙的情况
{ {
err = L_PSD - DIST_wall_RL; err = L_PSD - DIST_wall_RL;
PID_AL(); PID_AL();
} }
else if (20<R_PSD && R_PSD<140) //只有右侧有墙的情况 else if (20<R_PSD && R_PSD<150) //只有右侧有墙的情况
{ {
err = DIST_wall_RL - R_PSD; err = DIST_wall_RL - R_PSD;
PID_AL(); PID_AL();
} }
else else
{ {
L_PSD = L_PSD % 300; L_PSD = L_PSD % 100;
R_PSD = R_PSD % 300; R_PSD = R_PSD % 100;
err = L_PSD - R_PSD; err = L_PSD - R_PSD;
PID_AL(); PID_AL();
} }
VWGetPosition(&x_2, &y_2, &phi_2); VWGetPosition(&x_2, &y_2, &phi_2);
bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,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); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 }while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
//~ cout << "move:" << bot_move << endl;
MOTORDriveRaw(1,0); //~ MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); //~ MOTORDriveRaw(2,0);
} }

66
bot_code_turn/maze_func_turn.cpp
View File

@@ -8,71 +8,27 @@ using namespace std;
void BOTturn(int turn) void BOTturn(int turn)
{ {
int f_wall, b_wall, r_wall, l_wall;
int old_f_wall, old_b_wall;
int turn_check;
if (turn == -1) //右转 if (turn == -1) //右转
{
l_wall = PSDGetRaw(2) - 16;
if (l_wall < 145)
{
b_wall = PSDGetRaw(4) - 2;
do
{
old_b_wall = b_wall;
MOTORDriveRaw(1,19);
MOTORDriveRaw(2,-17);
OSWait(850);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变
turn_check = b_wall - old_b_wall;
OSWait(50);
} while ( turn_check < -3 || b_wall > 140);
}
else
{ {
ENCODERReset(1); ENCODERReset(1);
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,21); MOTORDriveRaw(1,22);
MOTORDriveRaw(2,-19); MOTORDriveRaw(2,-20);
} while (abs(ENCODERRead(2)) < 218); } while (abs(ENCODERRead(2)) < 250);
}
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
} }
else if (turn == 1) //左转 else if (turn == 1) //左转
{
r_wall = PSDGetRaw(3);
if (r_wall < 145)
{
b_wall = PSDGetRaw(4) - 2;
do
{
old_b_wall = b_wall;
MOTORDriveRaw(1,-19);
MOTORDriveRaw(2,17);
OSWait(850);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变
turn_check = b_wall - old_b_wall;
OSWait(50);
} while (turn_check < -3 || b_wall > 130);
}
else
{ {
ENCODERReset(1); ENCODERReset(1);
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,-21); MOTORDriveRaw(1,-22);
MOTORDriveRaw(2,19); MOTORDriveRaw(2,20);
} while (abs(ENCODERRead(2)) < 240); } while (abs(ENCODERRead(2)) < 245);
}
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
} }
@@ -82,8 +38,8 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,21); MOTORDriveRaw(1,22);
MOTORDriveRaw(2,-19); MOTORDriveRaw(2,-20);
} while (abs(ENCODERRead(2)) < 500); } while (abs(ENCODERRead(2)) < 500);
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
@@ -93,4 +49,10 @@ void BOTturn(int turn)
{ {
cout << "errer" << endl; cout << "errer" << endl;
} }
err_sum = 0;
err_old = 0;
err = 0;
err_diff = 0;
} }

24
bot_code_turn/maze_parameter.cpp
View File

@@ -9,9 +9,10 @@ using namespace std;
/*---定义数据---*/ /*---定义数据---*/
/*---定义全局常量---*/ /*---定义全局常量---*/
const int DIST_cell = 240; //单元格长度 const int DIST_cell = 240; //单元格长度
const int DIST_move = 292; //移动的单元格距离 const int DIST_move = 340; //移动的单元格距离
const int DIST_wall_F = 90; //与墙壁的距离 const int DIST_wall_F = 85; //与墙壁的距离
const int DIST_wall_RL = 88; //与左右墙壁的距离 //~ const int DIST_wall_RL = 90; //与左右墙壁的距离 1
const int DIST_wall_RL = 87; //与左右墙壁的距离
/*---定义全局变量---*/ /*---定义全局变量---*/
/*---容器类---*/ /*---容器类---*/
@@ -29,15 +30,20 @@ int rob_dir = 0; //机器人当前的朝向（初始为0）
/*---PID算法---*/ /*---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 = 2.8, Ki = 0.006, Kd = 0.08; //定义PD的参数16-2此方案成功过，就是有点摇摇晃晃 墙壁为1
//~ const float Kp = 0.80, Ki = 0.0076, Kd = 0.2; //定义PID的参数18 /*--在尝试的方案--*/
const float Kp = 2.82, Ki =0.006, Kd = 0.08; //定义PD的参数16-2
//~ const float Kp = 5, Ki = 0, Kd = 0.1; //定义PD的参数16-3
//~ const float Kp = 2.4, Ki = 0.006, Kd = 0.125; //定义PID的参数16-2
//~ const float Kp = 2.6, Ki = 0.006, Kd = 0.12; //定义PD的参数?
//~ const float Kp = 2.5, Ki = 0.01, Kd = 0.2; //定义PID的参数16-3
//~ float Kpid; //定义PID计算后输出的倍率 //~ float Kpid; //定义PID计算后输出的倍率
int Kpid_speed; int Kpid_speed;
const float Kpid_base = 1; //定义基本角速度和速度 const float Kpid_base = 1; //定义基本角速度和速度
const int speed_l = 16; //定义基本左轮速度 const int speed_l = 15; //定义基本左轮速度
//~ const int speed_r = speed_l - 2; //定义基本右轮速度 const int speed_r = speed_l - 1; //定义基本右轮速度
int speed_r = speed_l - 2; //定义基本右轮速度 //~ int speed_r = speed_l - 1; //定义基本右轮速度
const int speed_xianfu = 2; //定义kpid限幅 const int speed_xianfu = 2; //定义kpid限幅
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
const int eI_max = 200; //定义积分限幅 const int eI_max = 200; //定义积分限幅

4
bot_code_turn/maze_parameter.h
View File

@@ -31,8 +31,8 @@ extern const float Kp, Ki, Kd; //声明PID的参数
extern int Kpid_speed; //定义计算后的速度 extern int Kpid_speed; //定义计算后的速度
extern const float Kpid_base; //定义速度基础倍数速度 extern const float Kpid_base; //定义速度基础倍数速度
extern const int speed_l; //定义左轮基本速度 extern const int speed_l; //定义左轮基本速度
//~ extern const int speed_r; //定义右轮基本速度 extern const int speed_r; //定义右轮基本速度
extern int speed_r; //定义右轮基本速度 //~ extern int speed_r; //定义右轮基本速度
extern const int speed_xianfu; //kpid输出速度的限幅 extern const int speed_xianfu; //kpid输出速度的限幅
extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
extern const int eI_max; //定义积分限幅 extern const int eI_max; //定义积分限幅