Ejemplo n.º 1
0
 public void closeConnection()
 {
     instance = null;
     port.Close();
 }
Ejemplo n.º 2
0
        /**
         * start - Begins taking in CommandLine arguments and parses them
         *          into valid or invalid commands
         *
         * @return - none
         **/
        public void start()
        {
            // Start the Kinect
            startKinect();

            // Get the Arduino Port Object
            port = COMConnection.COMConnection.Instance;
            /*
            // Get the current relative angle from the IMU
            int startingDegree = getAverageCurrentLocationInDegrees();
            int degreeDifference = getDegreeAddition(startingDegree, initialDirectionDegree);

            Console.WriteLine("Degree IMU Needs to get to: " + degreeDifference);

            // Perform the initial Swivel to get to the appropriate starting direction
            // If less than 180 swivel right
            if (initialDirectionDegree <= 180)
            {
                port.sendString("N");
                port.sendString("30707");
                System.Threading.Thread.Sleep(500); // Let the wheels get going
            }
            else
            {
                port.sendString("N");
                port.sendString("40707");
                System.Threading.Thread.Sleep(500); // Let the wheels get going
            }

            // Until we get to the right direction keep swiveling
            while (!(withinDegrees(degreeDifference, startingDegree)))
            {
                startingDegree = getAverageCurrentLocationInDegrees();
                Console.WriteLine("New Degree: " + startingDegree);
            }
            */
            // Stop, wait 0.5 seconds
            port.sendString("N");
            port.sendString("51010");
            System.Threading.Thread.Sleep(500);

            while (true)
            {
                //System.Threading.Thread.Sleep(5000);
                // send mode
                //port.sendString(Console.ReadLine());

                // Make sure we are in Navigation Mode
                port.sendString("N");

                //System.Threading.Thread.Sleep(500);

               // string myString = Console.ReadLine();
                // send right ticks
               // port.sendString(Console.ReadLine());

                // send left ticks
                //port.sendString(Console.ReadLine());

                // Check for first threshhold
                if (walkerKinect.isEmergency())
                {
                    int turnResult = walkerKinect.isRightTurnBetter();
                    switch (turnResult)
                    {
                        case 1:
                            Console.WriteLine("Go Straight Slowly...");
                            goStraightSlowly();
                            break;
                        case 2:
                            Console.WriteLine("Stop, start turning LEFT!");
                            pivotLeft();
                            break;
                        case 3:
                            Console.WriteLine("Stop, start turning RIGHT!");
                            pivotRight();
                            break;
                        default:
                            Console.WriteLine("SWIVEL!");
                            //walkerKinect.setYPos(((leftEncoderTick + rightEncoderTick / 2) * mmPerEncoder) / 20);
                            //walkerKinect.printMap();

                            swivelRight();
                            break;
                    }
                }
                // Check for Second Threshold
                else if (walkerKinect.isBlocked())
                {
                    int turnResult = walkerKinect.isRightTurnBetter();
                    switch (turnResult)
                    {
                        case 1:
                            Console.WriteLine("Go Straight Slowly...");
                            goStraightSlowly();
                            break;
                        case 2:
                           Console.WriteLine("Start turning slowly LEFT!");
                            glideLeft();
                            break;
                        case 3:
                            Console.WriteLine("Start turning slowly RIGHT!");
                            glideRight();
                            break;
                        default:
                            Console.WriteLine("SWIVEL!");
                            //walkerKinect.setYPos(((leftEncoderTick + rightEncoderTick / 2) * mmPerEncoder) / 20);
                            //walkerKinect.printMap();

                            swivelRight();
                            break;
                    }
                }
                else
                {
                    // If there are no problems just keep going straight
                    //Console.WriteLine("KEEP GOING STRAIGHT!");
                    goStraight();
                }
                //port.readLineString();
                // Send the determined string to the Arduino for the motors
                port.sendString(motorControlString);
                System.Threading.Thread.Sleep(500);

                //port.clearStream();
                /*
                string leftTick = "";
                string rightTick = "";

                port.sendString("F");
                    leftTick = port.readLineString();
                    //Console.WriteLine("LEFT: " + leftTick);
                    //leftEncoderTick = tick;
                    /*byte[] asciiBytes = Encoding.ASCII.GetBytes(leftTick);
                    Console.WriteLine("Left ASCII Values");
                    foreach (byte b in asciiBytes)
                    {
                        Console.Write(b);
                    }
                     * */
                    //Console.WriteLine();

                //System.Threading.Thread.Sleep(200);

               /* port.sendString("H");

                    rightTick = port.readLineString();

                Console.WriteLine("Right Ticks: " + rightTick);
                Console.WriteLine();

                try
                {
                    leftEncoderTick = long.Parse(leftTick);
                }
                catch (Exception e)
                {
                    Console.WriteLine("ERROR LEFT");
                }

                try
                {
                    rightEncoderTick = long.Parse(rightTick);
                }
                catch (Exception e)
                {
                    Console.WriteLine("ERROR RIGHT");
                }

                Console.WriteLine("NEW TICKS");
                Console.WriteLine("Left New: " + leftEncoderTick);
                Console.WriteLine("Right New: " + rightEncoderTick);

               // System.Threading.Thread.Sleep(500);
                */
            }

            // Close the connection to the Arduino
            port.closeConnection();
        }
Ejemplo n.º 3
0
 public ForceCommand()
 {
     port = COMConnection.COMConnection.Instance;
 }
Ejemplo n.º 4
0
 public StrainCommand()
 {
     port = COMConnection.COMConnection.Instance;
 }