public void Discover()
{
currentDegToOpp = Mathematics.getSlope(p1, p2);
double curr = Mathematics.Degree(OwnRobots.CurrentRobot);
//double Blockdist = Mathematics.Distance(p1, Objects.Block);
slope = Mathematics.DegreeToOpp(OwnRobots.CurrentRobot, OppRobots.CurrentRobot);
ExploreCommander.GoForward();
// ExploreCommander.Turn(slope, Command.Forward);
ExploreCommander.Turn(curr + 45, Command.Forward);
Thread.Sleep(400);
while (true)
{
Dist = Mathematics.Distance(OwnRobots.CurrentRobot, OppRobots.CurrentRobot);
p1 = OwnRobots.CurrentRobot;
p2 = OppRobots.CurrentRobot;
slope = Mathematics.DegreeToOpp(OwnRobots.CurrentRobot, OppRobots.CurrentRobot);
if (Consts.LastCommand != Command.Forward)
{
ExploreCommander.GoForward();
Consts.LastCommand = Command.Forward;
}
ExploreCommander.Turn(slope, Command.Forward);
Follow();
}
}
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());
}
private static void SelectedCommand(Command cmd)
{
switch (cmd)
{
case Command.Forward:
ExploreCommander.GoForward();
break;
case Command.Backward:
ExploreCommander.GoBackward();
break;
}
}
public static void TurnRight(double deg)
{
double cdegree = Mathematics.Degree(OwnRobots.CurrentRobot);
if (cdegree < deg)
{
if (Consts.LastCommand != Command.TurnRight)
{
ExploreCommander.TurnRight();
Consts.LastCommand = Command.TurnRight;
}
}
}
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");
// }
//}
}