Ejemplo n.º 1
0
        public void run()
        {
            // prev and new reading store short range sensor distances
            double prevReading = 0;
            double newReading  = 0;

            // State of the agent
            EState state = EState.NONE;

            // Stopwatch to measure the amount of time spent in each mode
            Stopwatch stopwatch  = new Stopwatch();
            long      randomTime = 0;
            long      wallTime   = 0;

            // Agent loop
            while (true)
            {
                // find what to do

                /*if (wallTime > randomTime)
                 *  state = EState.RANDOM;
                 * else*/
                state = EState.WALLFOLLOWING;
                //state = EState.RANDOM;

                // do it
                stopwatch.Start();
                if (state == EState.RANDOM)
                {
                    while (bumperSensor.collide())
                    {
                        // spin left
                        movement.spinLeft(45, random.Next(0, 360));
                        cleaner.clean();
                    }

                    while (!bumperSensor.collide())
                    {
                        // move straight at 10cm/s
                        movement.straight(10);
                        cleaner.clean();
                    }
                }

                else if (state == EState.WALLFOLLOWING)
                {
                    // while against a wall turn left
                    while (bumperSensor.collide())
                    {
                        movement.spinLeft(45, 1);
                        cleaner.clean();
                    }

                    // start following the wall adjusting the angle
                    prevReading = ShortRangeSensor.getDistance();
                    movement.spinLeft(45, 1);
                    while (!bumperSensor.collide())
                    {
                        double alpha;

                        movement.straight(10);
                        cleaner.clean();
                        newReading = shortRangeSensor.getDistance();

                        // if no previous reading do not modify the angle
                        if (prevReading == -1)
                        {
                            alpha = 0;
                        }
                        // if we are not against a wall anymore do a 90 degree turn
                        // to look for the wall
                        else if (newReading == -1)
                        {
                            for (int i = 0; i < 10; i++)
                            {
                                if (bumperSensor.collide())
                                {
                                    break;
                                }
                                movement.straight(10);
                                cleaner.clean();
                            }
                            alpha = 90;
                        }
                        // if we got the previous and current reading calculate the inverse tangent
                        // of the angle that would put the agent in the right angle
                        else
                        {
                            alpha = Math.Atan((newReading - prevReading) / 1) * 180 / Math.PI;
                        }

                        movement.spinRight(45, alpha);
                        cleaner.clean();

                        prevReading = newReading;
                    }
                }
                stopwatch.Stop();

                // allocate the time spent
                if (state == EState.WALLFOLLOWING)
                {
                    wallTime += stopwatch.ElapsedTicks;
                }
                else
                {
                    randomTime += stopwatch.ElapsedTicks;
                }
            }
        }
Ejemplo n.º 2
0
        public void run()
        {
            double prevReading = 0;
            double newReading  = 0;

            // Wall following
            while (!gohome)
            {
                // Go to the wall
                while (!longRangeSensor.againstWall())
                {
                    movement.straight(1);
                    distance += 1;
                    System.Threading.Thread.Sleep(5);
                }

                nodes.Insert(distance, angle, 5);
                distance = 0;

                // While the agent is touching a wall
                while (longRangeSensor.againstWall())
                {
                    movement.spinLeft(1, 1);
                    angle = (angle - 1) % 360;
                    System.Threading.Thread.Sleep(5);
                }

                // start following the wall adjusting the angle
                prevReading = ShortRangeSensor.getDistance();
                while (!longRangeSensor.againstWall())
                {
                    movement.straight(1);
                    distance  += 1;
                    newReading = shortRangeSensor.getDistance();

                    double alpha;

                    if (prevReading == -1)
                    {
                        alpha = 0;
                    }
                    else if (newReading == -1)
                    {
                        alpha = 90;
                    }
                    else
                    {
                        alpha = Math.Atan((newReading - prevReading) / 1) * 180 / Math.PI;
                        if (Math.Abs(alpha) < 5)
                        {
                            alpha = 0;
                        }
                    }

                    if (alpha != 0)
                    {
                        //no more wall
                        nodes.Insert(distance, angle, 5);
                        distance = 0;
                    }

                    movement.spinRight(1, alpha);
                    angle = (angle + alpha) % 360;

                    prevReading = newReading;

                    System.Threading.Thread.Sleep(5);
                }
            }

            nodes.Insert(distance, angle, 5);
            distance = 0;

            while (nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
            {
                // Go home
                // point in direction to the start
                Point A     = new Point(0, 0);
                Point C     = nodes.CurrentPosition(distance, angle);
                Point Btemp = C + new Point(0, 100);
                Point B     = new Segment(C, Btemp).Rotate(angle).End;

                double a_sq = Math.Pow(100, 2);
                double b_sq = A.SquareDistance(C);
                double c_sq = A.SquareDistance(B);

                double gamma;

                gamma = Math.Acos((c_sq - a_sq - b_sq) / (-2 * Math.Sqrt(a_sq) * Math.Sqrt(b_sq))) * 180 / Math.PI;

                if (b_sq > c_sq)
                {
                    gamma = 90 - gamma;
                }

                if (angle < 0)
                {
                    angle = (360 + angle);
                }

                if (angle < 180)
                {
                    movement.spinRight(1, gamma);
                    angle = (angle + gamma) % 360;
                }
                else
                {
                    movement.spinLeft(1, gamma);
                    angle = (angle - gamma) % 360;
                }

                // no wall let's move straight
                while (longRangeSensor.getDistance() == -1)
                {
                    movement.straight(1);
                    distance++;
                    System.Threading.Thread.Sleep(5);
                }

                // A wall is in range
                Point   pos    = nodes.CurrentPosition(distance, angle);
                Segment sensor = new Segment(pos, new Point(longRangeSensor.getDistance(), 0)).Rotate(angle);
                BDNode  near   = nodes.Nearest(sensor.End, 1)[0];
                if (near.Next != null && new Segment(near.Coord, near.Next.Coord).Intersects(sensor) != null)
                {
                    ;//MessageBox.Show("intersect");
                }
                else if (near.Previous != null && new Segment(near.Coord, near.Previous.Coord).Intersects(sensor) != null)
                {
                    ;//MessageBox.Show("intersect");
                }
                else
                {
                    ;// MessageBox.Show("unknown wall");
                }
                // move straight until hit the wall
                while (!longRangeSensor.againstWall() && nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
                {
                    movement.straight(1);
                    distance++;
                    System.Threading.Thread.Sleep(5);
                }

                nodes.Insert(distance, angle, 5);
                distance = 0;

                // While the agent is touching a wall
                while (longRangeSensor.inRange())
                {
                    movement.spinLeft(1, 1);
                    angle = (angle - 1) % 360;
                    System.Threading.Thread.Sleep(5);
                }
                while (shortRangeSensor.inRange())
                {
                    movement.straight(1);
                    distance++;
                    System.Threading.Thread.Sleep(5);
                }
                nodes.Insert(distance, angle, 5);
                distance = 0;
            }
        }