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(); } }