public static void Turn(double degree, Command command)
{
double currentDegree = Mathematics.Degree(OwnRobots.CurrentRobot);
if (degree < currentDegree)
{
while (degree <= currentDegree)
{
currentDegree = Mathematics.Degree(OwnRobots.CurrentRobot);
ExploreCommander.TurnLeft(degree);
}
}
else
{
while (degree >= currentDegree)
{
currentDegree = Mathematics.Degree(OwnRobots.CurrentRobot);
ExploreCommander.TurnRight(degree);
}
}
ExploreCommander.Stop();
ExploreCommander.SelectedCommand(command);
Console.WriteLine(Mathematics.Degree(OwnRobots.CurrentRobot).ToString());
}
public void Follow()
{
if (Dist < 400)
{
if (Consts.LastCommand != Command.Backward)
{
ExploreCommander.GoBackward();
Consts.LastCommand = Command.Backward;
}
Thread.Sleep(200);
}
if (Mathematics.IsEqual(slope, Mathematics.Degree(OwnRobots.CurrentRobot), 4))
{
if (Consts.LastCommand != Command.Stop)
{
ExploreCommander.Stop();
//ExploreCommander.GoForward();
Consts.LastCommand = Command.Stop;
}
}
if (!Mathematics.IsEqual(slope, Mathematics.Degree(OwnRobots.CurrentRobot), 4))
{
ExploreCommander.Turn(slope, Command.Stop);
}
if (Dist < 600 && !Mathematics.IsEqual(slope, Mathematics.Degree(OwnRobots.CurrentRobot), 4))
{
ExploreCommander.Turn(slope, Command.Stop);
}
//else if (Mathematics.IsEqual(slope, Mathematics.Degree(OwnRobots.CurrentRobot), 4) && Dist < 700)
//{
// if (Consts.LastCommand != Command.Stop)
// {
// ExploreCommander.Stop();
// Consts.LastCommand = Command.Stop;
// Console.WriteLine("deg==curr && dist<700");
// }
//}
}