void PrgReflectBorder()
{
Random ran = new Random();
TurnRelAngle(ran.Next(-180, 180), 5);
while (true)
{
if (rb.Pos.X < DblBuffForm.XMin())
{
DriveDistance(-5, 20);
TurnRelAngle(rb.GyroAngle > 0 ? -90 : 90, 5);
}
else if (rb.Pos.X > DblBuffForm.XMax())
{
DriveDistance(-5, 20);
TurnRelAngle(rb.GyroAngle > 0 ? 90 : -90, 5);
}
else if (rb.Pos.Y < DblBuffForm.YMin())
{
DriveDistance(-5, 20);
TurnRelAngle(rb.GyroAngle + 90 > 0 ? 90 : -90, 5);
}
else if (rb.Pos.Y > DblBuffForm.YMax())
{
DriveDistance(-5, 20);
double phi = rb.GyroAngle;
TurnRelAngle(rb.GyroAngle - 90 > 0 ? 90 : -90, 5);
}
rb.SetV(5);
WaitForUpdate();
}
}
void ProgDoge()
{
const double speed = 5;
Random ran = new Random();
DogeState state = DogeState.Turn;
double vFactor = 1;
double v = 0;
double turnedPhi = 0;
double relPhi = 0;
TurnAbsAngle(ran.Next(-180, 180), 5);
double previousPhi = GetAngleInRange(rb.GyroAngle);
while (true)
{
double phi = GetAngleInRange(rb.GyroAngle);
if (rb.Pos.X < DblBuffForm.XMin())
{
if (phi >= -180 && phi <= -90 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 90 && phi <= 180 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= 0 && phi <= 90 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -90 && phi <= 0 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else if (rb.Pos.X > DblBuffForm.XMax())
{
if (phi >= 0 && phi <= 90 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -90 && phi <= 0 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= -180 && phi <= -90 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 90 && phi <= 180 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else if (rb.Pos.Y < DblBuffForm.YMin())
{
if (phi >= -90 && phi <= 0 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -180 && phi <= -90 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= 90 && phi <= 180 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 0 && phi <= 90 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else if (rb.Pos.Y > DblBuffForm.YMax())
{
if (phi >= 90 && phi <= 180 && v > 0 && state != DogeState.BackwardClockwise)
{
state = DogeState.BackwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= 0 && phi <= 90 && v > 0 && state != DogeState.BackwardAnticlockwise)
{
state = DogeState.BackwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
else if (phi >= -90 && phi <= 0 && v < 0 && state != DogeState.ForwardClockwise)
{
state = DogeState.ForwardClockwise;
relPhi = 45;
turnedPhi = 0;
}
else if (phi >= -180 && phi <= -90 && v < 0 && state != DogeState.ForwardAnticlockwise)
{
state = DogeState.ForwardAnticlockwise;
relPhi = -45;
turnedPhi = 0;
}
}
else
{
Robot[] nearRobots = RobotMgr.GetRobots().Where(r => r != rb && DistanceTo(r) < 50).
OrderBy(r => DistanceTo(r)).ToArray();
int toNearRobotsCount = nearRobots.Count(r => DrivingTowardsOther(r));
if (toNearRobotsCount > 0)
{
state = DogeState.Turn;
double toRobotPhi = (nearRobots.First().Pos - rb.Pos).GetPhiGrad();
double relToRobotPhi = GetRelAngle(toRobotPhi);
relPhi = relToRobotPhi < 0 ? 180 : -180;
turnedPhi = 0;
}
else if (toNearRobotsCount == 0 && state == DogeState.Turn)
{
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
}
vFactor = nearRobots.Length > 0 ? Math.Pow(DistanceTo(nearRobots.First()) / 50, 3) : 1;
}
switch (state)
{
case DogeState.Forward:
rb.SetV(v = speed * vFactor);
break;
case DogeState.ForwardClockwise:
rb.SetV(v = speed * vFactor);
break;
case DogeState.ForwardAnticlockwise:
rb.SetV(v = speed * vFactor);
break;
case DogeState.BackwardClockwise:
rb.SetV(v = -speed * vFactor);
break;
case DogeState.BackwardAnticlockwise:
rb.SetV(v = -speed * vFactor);
break;
case DogeState.Turn:
rb.SetV(v = 0);
break;
}
if (SetTurnRelAngle(relPhi, 5, ref turnedPhi, ref previousPhi))
{
switch (state)
{
case DogeState.ForwardClockwise:
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
break;
case DogeState.ForwardAnticlockwise:
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
break;
case DogeState.BackwardClockwise:
state = DogeState.ForwardAnticlockwise;
relPhi = 45;
turnedPhi = 0;
break;
case DogeState.BackwardAnticlockwise:
state = DogeState.ForwardClockwise;
relPhi = -45;
turnedPhi = 0;
break;
case DogeState.Turn:
state = DogeState.Forward;
relPhi = 0;
turnedPhi = 0;
break;
}
}
WaitForUpdate();
}
}