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.17第一次提交，包括16号和今天，在实机上，对代码修改后的结果

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-17 15:04:27 +08:00
parent 137218ebe5
commit 5ce7796b14
7 changed files with 300 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
robot_test_code/LCD输出测试/makefile Normal file
View 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

105
robot_test_code/PID/PID.cpp
View File

@@ -4,24 +4,27 @@
#include"PID.h"
using namespace std;
float Kp = 0.2, Ki = 0.1, Kd = 0.1; //定义PID的参数
float Kp = 0.79, Ki = 0.009, Kd = 0.14; //定义PID的参数
float Kpid;
int Kpid_speed;
const int speed = 18;
const float Kpid_base = 0.4;
const int speed_l = 17;
const int speed_r = speed_l - 2;
const int speed_err = 2;
const float Kpid_base = 1;
// const int speed2 = 0.75*speed;
// int speed1;
//int speed2 = speed;
/*---定义全局变量---*/
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
const int eI_max = 20;
const int eI_max = 200;
/*---定义全局常量---*/
/*---定义全局常量---*/
// const int DIST_cell = 300 ;
// const int DIST_wall = 100 ;
const int DIST_cell = 300;
const int DIST_wall_f = 80;
const int DIST_wall_rl = 87;
const int DIST_cell = 310;
const int DIST_wall_f = 120;
const int DIST_wall_rl = 90;
/*---全局各种变量与常量定义完成---*/
@@ -58,13 +61,13 @@ void PID_AL()
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
Kpid_speed = Kpid * Kpid_base;
if (Kpid_speed >= 6) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
if (Kpid_speed >= speed_err) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
{
Kpid_speed = 6;
Kpid_speed = speed_err;
}
else if (Kpid_speed <= -6)
else if (Kpid_speed <= -speed_err)
{
Kpid_speed = -6;
Kpid_speed = -speed_err;
}
else
{
@@ -74,8 +77,9 @@ void PID_AL()
cout << "PID_out:" << Kpid_speed << " " ;
/*---打印结束---*/
MOTORDriveRaw(2,speed + Kpid_speed); //控制小车的行驶速度，角速度
MOTORDriveRaw(1,speed);
MOTORDriveRaw(2,Kpid_speed + speed_r); //控制小车的行驶速度，角速度
MOTORDriveRaw(1,speed_l);
}
/*---PID控制下的直线行驶---*/
@@ -83,48 +87,56 @@ 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的参数定义
//~ 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) - 18;
// 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(1,speed);
MOTORDriveRaw(2,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); //读取前方和墙壁的距离
// F_PSD = F_PSD * 0.4 + F_PSD_old * 0.6;
// F_PSD_old = F_PSD;
L_PSD = PSDGetRaw(2) - 18; //读取左侧和墙壁的距离
// L_PSD = L_PSD * 0.4 + L_PSD_old * 0.6;
// L_PSD_old = L_PSD;
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 = R_PSD * 0.4 + R_PSD_old * 0.6;
// R_PSD_old = R_PSD;
//~ 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 (25<L_PSD && L_PSD<180 && 50<R_PSD && R_PSD<180) //如果两侧都有墙的情况
if (20<L_PSD && L_PSD<180 && 50<R_PSD && R_PSD<180) //如果两侧都有墙的情况
{
err = L_PSD - R_PSD;
PID_AL();
}
else if (25<L_PSD && L_PSD<180) //只有左侧有墙的情况
else if (20<L_PSD && L_PSD<180) //只有左侧有墙的情况
{
err = L_PSD - DIST_wall_rl;
PID_AL();
}
else if (25<R_PSD && R_PSD<180) //只有右侧有墙的情况
else if (20<R_PSD && R_PSD<180) //只有右侧有墙的情况
{
err = DIST_wall_rl - R_PSD;
PID_AL();
@@ -136,16 +148,25 @@ void PIDStraight()
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;
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);

65
robot_test_code/PID/PIDmain.cpp
View File

@@ -8,45 +8,62 @@ using namespace std;
// const int DIST_cell = 300 ;
// const int DIST_wall = 100 ;
const int DIST_cell = 300;
const int DIST_wall_f = 80;
const int DIST_wall_rl = 87;
const int DIST_wall_f = 120;
const int DIST_wall_rl = 90;
/*---全局各种变量与常量定义完成---*/
/*---下面是主函数---*/
int main()
{
int Fwall,Lwall,Rwall;
//~ 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; //检测右面
//判断旋转方向
if (Lwall)
//~ 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)
{
VWTurn(+90,15); //向左旋转
VWWait();
goto end_main;
}
else if (Fwall)
//~ //判断旋转方向
//~ 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)
{
//不旋转
}
else if (Rwall)
{
VWTurn(-90,15); //向右旋转
VWWait();
}
else
{
VWTurn(180,15);
VWWait();
}
//直走
PIDStraight();
}
//~ PIDStraight();
VWWait();
} while (KEYRead() != KEY4);
end_main:
cout << "over" << endl;
}

26
robot_test_code/实机测试/makefile Normal file
View File

@@ -0,0 +1,26 @@
cxx := gccarm
#~ file_cpp := $(wildcard *.cpp)
file_cpp := ./test_turn.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
View 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); //检测前面侧是否有墙
Lwall = PSDGetRaw(2); //检测左面
Rwall = PSDGetRaw(3); //检测右面
bwall = PSDGetRaw(4); //
cout << "PSD L:" << Lwall << " R:" << Rwall << " F:" << Fwall << " b:" << bwall << endl; ;
} while (KEYRead() != KEY4);
}

24
robot_test_code/实机测试/test_run.cpp
View File

@@ -7,13 +7,27 @@ using namespace std;
int main()
{
int Fwall,Lwall,Rwall;
LCDMenu("","","","END");
//~ int num;
do
{ //检测墙面
MOTORDriveRaw(1,50);
MOTORDriveRaw(2,50);
{
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);
}

106
robot_test_code/实机测试/test_turn.cpp Normal file
View File

@@ -0,0 +1,106 @@
#include<iostream>
#include<string>
#include"eyebot++.h"
using namespace std;
int main()
{
//~ int f_wall, r_wall, l_wall, b_wall;
//~ int old_f_wall, old_r_wall, old_l_wall, old_b_wall;
int b_wall;
int 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);
}
//~ 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);
}