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

66
bot_code_turn/maze_func_turn.cpp
View File

@@ -8,71 +8,27 @@ using namespace std;
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) //右转
{
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(2);
do
{
MOTORDriveRaw(1,21);
MOTORDriveRaw(2,-19);
} while (abs(ENCODERRead(2)) < 218);
}
MOTORDriveRaw(1,22);
MOTORDriveRaw(2,-20);
} while (abs(ENCODERRead(2)) < 250);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
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(2);
do
{
MOTORDriveRaw(1,-21);
MOTORDriveRaw(2,19);
} while (abs(ENCODERRead(2)) < 240);
}
MOTORDriveRaw(1,-22);
MOTORDriveRaw(2,20);
} while (abs(ENCODERRead(2)) < 245);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
@@ -82,8 +38,8 @@ void BOTturn(int turn)
ENCODERReset(2);
do
{
MOTORDriveRaw(1,21);
MOTORDriveRaw(2,-19);
MOTORDriveRaw(1,22);
MOTORDriveRaw(2,-20);
} while (abs(ENCODERRead(2)) < 500);
MOTORDriveRaw(1,0);
@@ -93,4 +49,10 @@ void BOTturn(int turn)
{
cout << "errer" << endl;
}
err_sum = 0;
err_old = 0;
err = 0;
err_diff = 0;
}