58 lines
1.1 KiB
C++
58 lines
1.1 KiB
C++
#include<iostream>
|
|
#include<string>
|
|
#include<cstdlib>
|
|
#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;
|
|
exit(1);
|
|
}
|
|
err_sum = 0;
|
|
err_old = 0;
|
|
err = 0;
|
|
err_diff = 0;
|
|
}
|