Beispiel #1
0
        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();
            }
        }
Beispiel #2
0
        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();
            }
        }