示例#1
0
文件: Brain.cs 项目: minhnh/sos-sub
        void Bump()
        {
            Point blobCenter = new Point(0, 0);

            currentDirection = STOPPED;
            DirectMotorLink.KillAllMotors();
            while (blobCenter.X > Int16.MinValue)//min value is returned when nothing can be seen
            {
                blobCenter = Eyes.GetCenterMass(true);
                if (blobCenter.X < -10)
                {
                    if (currentDirection != LEFT)
                    {
                        DirectMotorLink.KillAllMotors();
                        DirectMotorLink.YawLeft();
                        DirectMotorLink.DriveLeft();
                        currentDirection = LEFT;
                    }
                }
                else if (blobCenter.X > 10)
                {
                    if (currentDirection != RIGHT)
                    {
                        DirectMotorLink.KillAllMotors();
                        DirectMotorLink.YawRight();
                        DirectMotorLink.DriveRight();
                        currentDirection = RIGHT;
                    }
                }
                else if (blobCenter.Y < -10)
                {
                    if (currentDirection != UP)
                    {
                        DirectMotorLink.KillAllMotors();
                        DirectMotorLink.Surface();
                        currentDirection = UP;
                    }
                }
                else if (blobCenter.Y > 10)
                {
                    if (currentDirection != DOWN)
                    {
                        DirectMotorLink.KillAllMotors();
                        DirectMotorLink.Dive();
                        currentDirection = DOWN;
                    }
                }
                else
                {
                    if (currentDirection != FORWARD)
                    {
                        DirectMotorLink.KillAllMotors();
                        DirectMotorLink.DriveForward();
                        currentDirection = FORWARD;
                    }
                }
                Thread.Sleep(50);//only do something new every half second
            }
            DirectMotorLink.KillAllMotors();
        }
示例#2
0
文件: Brain.cs 项目: minhnh/sos-sub
        void FollowLine()
        {
            Point blobCenter = new Point(0, 0);

            currentDirection = STOPPED;
            //DirectMotorLink.KillAllMotors();
            //DirectMotorLink.DriveForward();



            // DirectMotorLink.Surface();
            //DirectMotorLink.KillAllMotors();
            Console.WriteLine("Diving: 2.25 seconds.\n");
            DirectMotorLink.Dive();
            Thread.Sleep(9000);
            Console.WriteLine("Killing motors.\n");
            DirectMotorLink.KillAllMotors();

            for (int i = 0; i < 10; i++)
            {
                DirectMotorLink.DriveBackwards();
                DirectMotorLink.KillAllMotors();
                Console.WriteLine("Forward: 10 seconds.\n");
                DirectMotorLink.DriveForward();
                Thread.Sleep(10000);
                Console.WriteLine("Killing motors.\n");
                DirectMotorLink.KillAllMotors();

                //if (i == 3)
                //{
                //    Console.WriteLine("Surfacing: 1.75 seconds.\n");
                //    DirectMotorLink.Surface();
                //    Thread.Sleep(500);
                //}


                // DirectMotorLink.Dive();
                //DirectMotorLink.KillAllMotors();
                // DirectMotorLink.Surface();
                //Thread.Sleep(2200);
                //DirectMotorLink.KillAllMotors();

                //Console.WriteLine("Rotating Right");
                //DirectMotorLink.YawRight();
                //Console.WriteLine("Killing");
                //DirectMotorLink.KillAllMotors();
                //Console.WriteLine("Diving: 1 second.\n");
                //DirectMotorLink.Dive();
                //Thread.Sleep(2000);
                DirectMotorLink.Surface();
                Thread.Sleep(500);
                Console.WriteLine("Turning left: 2.4 seconds.\n");
                DirectMotorLink.YawRight();
                DirectMotorLink.KillAllMotors();
                DirectMotorLink.YawLeft();
                Thread.Sleep(3500);
                Console.WriteLine("Killing motors.\n");
                DirectMotorLink.KillAllMotors();
                Console.WriteLine("Finished cycle" + (i + 1) + ". Starting cycle " + (i + 2) + ".\n");
            }
            Console.WriteLine("\nRun Finished, switching to downward vision.\n");

            //DirectMotorLink.KillAllMotors();
            //KillEverything();



            do//move until a blob is seen
            {
                blobCenter = Eyes.GetCenterMass(false);
            } while (blobCenter.X == Int32.MinValue);
            DirectMotorLink.KillAllMotors();
            Console.WriteLine("Killing motors.\n");

            while (blobCenter.X > Int16.MinValue)//min value is returned when nothing can be seen
            {
                blobCenter = Eyes.GetCenterMass(false);
                if (blobCenter.X < -10)
                {
                    if (currentDirection != LEFT)
                    {
                        Console.WriteLine("Killing motors.\n");
                        DirectMotorLink.KillAllMotors();
                        //DirectMotorLink.YawLeft();
                        Console.WriteLine("Turning left for .95 seconds.\n");
                        DirectMotorLink.DriveLeft();
                        Thread.Sleep(950);
                        Console.WriteLine("Driving forward for 5 seconds.\n");
                        DirectMotorLink.DriveForward();
                        Thread.Sleep(5000);
                        currentDirection = LEFT;
                    }
                }
                else if (blobCenter.X > 10)
                {
                    if (currentDirection != RIGHT)
                    {
                        Console.WriteLine("Killing motors.\n");
                        DirectMotorLink.KillAllMotors();
                        //DirectMotorLink.YawRight();
                        Console.WriteLine("Driving right for .95 seconds.\n");
                        DirectMotorLink.DriveRight();
                        Thread.Sleep(950);
                        Console.WriteLine("Driving for 5 seconds.\n");
                        DirectMotorLink.DriveForward();
                        Thread.Sleep(5000);
                        currentDirection = RIGHT;
                    }
                }
                else
                {
                    if (currentDirection != FORWARD)
                    {
                        Console.WriteLine("Killing motors.\n");
                        DirectMotorLink.KillAllMotors();
                        Console.WriteLine("Driving forward for 5 seconds.\n");
                        DirectMotorLink.DriveForward();
                        Thread.Sleep(5000);
                        currentDirection = FORWARD;
                    }
                }
                Thread.Sleep(50);//only do something new every half second
            }
            Console.WriteLine("Killing motors.\n");
            DirectMotorLink.KillAllMotors();
        }