turnLeft() private method

private turnLeft ( ) : void
return void
Ejemplo n.º 1
0
        public void Act()
        {
            while (!end)
            {
                if (alarm)
                {
                    lock (monitor)
                    {
                        if (count >= 5)
                        {
                            Engine.getInstance().Debug("Contingency count: " + count);
                            if (count >= 10)
                            {
                                navigation.turnLeft(20, (sbyte)90);
                            }
                            else if (count >= 15)
                            {
                                navigation.turnRight(20, (sbyte)90);
                                count = 10;
                            }
                            else
                            {
                                navigation.MoveForward(100, (sbyte)90);
                            }
                        }
                    }
                    Engine.getInstance().Debug("Solving problem when moving " + movement.ToString());
                    switch (movement)
                    {
                    case Movement.forward:
                        navigation.MoveBackward(100, (sbyte)40);
                        break;

                    case Movement.backward:
                        navigation.MoveForward(100, (sbyte)40);
                        break;

                    case Movement.left:
                        navigation.turnRight(10, (sbyte)40);
                        break;

                    case Movement.right:
                        navigation.turnLeft(10, (sbyte)50);
                        break;
                    }

                    Engine.getInstance().Restart();
                    alarm = false;
                }
            }
        }
Ejemplo n.º 2
0
        private void _Run()
        {
            while (!cancel)
            {
                debug.Write(true);
                Thread.Sleep(50);
                debug.Write(false);

                if (currentMode == Mode.SearchingForWall)
                {
                    navigation.MoveToObject(sensors);

                    float central = sensors.getDistance(Sensor.Central);

                    if (central <= GlobalVal.distanceToDetect)
                    {
                        //WALL FOUND
                        navigation.TurnRightUntilWall(sensors);
                        currentMode = Mode.FollowWall;
                        Info("Current mode: Follow Wall");
                    }
                    continue;
                }
                else if (currentMode == Mode.FollowWall)
                {
                    float  central;
                    float  wall;
                    float  wallback;
                    double error_distance, angle_error;

                    //FOLLOW LEFT WALL
                    while (!cancel)
                    {
                        wall           = sensors.getDistance(Sensor.Wall);
                        wallback       = sensors.getDistance(Sensor.wall_back);
                        central        = sensors.getDistance(Sensor.Central);
                        error_distance = 0;
                        angle_error    = 0;

                        if (central < GlobalVal.distanceToDetect)
                        {
                            break;
                        }

                        if (exMath.Abs(wall - wallback) < GlobalVal.hysteresis)
                        {
                            if (wall < 30 && wallback < 30)
                            {
                                //Maintain certain distance to the wall (too close may be a problem)
                                if (wall < GlobalVal.minDistanceToFollowWall || wallback < GlobalVal.minDistanceToFollowWall)
                                {
                                    if (wall <= wallback)
                                    {
                                        navigation.turnRight(10);
                                    }
                                }

                                navigation.MoveForward(20);
                            }
                            else
                            {
                                continue;
                            }
                        }//CORRECT THE DEVIATION RESPECT TO THE WALL
                        else if (wall < wallback)
                        {
                            error_distance = wall - wallback;

                            if (error_distance < 5)
                            {
                                angle_error = exMath.Atan2(error_distance * 10, GlobalVal.length_mm);
                                navigation.turnLeft((int)(angle_error / 2));
                                navigation.MoveForward(30);//50
                            }
                            else//THERE IS A CORNER
                            {
                                Debug("LEFT CORNER");
                                navigation.TurnLeftUntilWall(sensors);
                            }
                        }//IN THE FOLLOWING CASE:
                        //1-IS A WALL DEVIATION
                        //2-THERE IS A CORNER
                        else if (wall > wallback)
                        {
                            if ((wall - wallback) < 5)
                            {
                                navigation.turnLeft(1);
                                navigation.MoveForward(50);
                            }
                            else//THERE IS A CORNER
                            {
                                Debug("LEFT CORNER");
                                navigation.TurnLeftUntilWall(sensors);
                            }
                        }
                    }
                    if (!cancel)
                    {
                        navigation.TurnRightUntilWall(sensors);
                    }
                }
            }
        }