コード例 #1
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
        public void setCurrentDriveType(int newStatus)
        {
            switch (newStatus)
            {
            case 0:
                CurrentDriveType = DriveTypeStatus.UserDriven;
                break;

            case 1:
                CurrentDriveType = DriveTypeStatus.UnRestrictedUserDriven;
                break;

            case 2:
                CurrentDriveType = DriveTypeStatus.RoutDriven;
                break;

            case 3:
                CurrentDriveType = DriveTypeStatus.DoorDriven;
                break;

            case 4:
                CurrentDriveType = DriveTypeStatus.ProgramDriven;
                break;

            case 5:
                CurrentDriveType = DriveTypeStatus.ObjectDetectedDriven;     // ADDED for when an object is detected
                break;
            }
        }
コード例 #2
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
 //sets the Route and path to empty and sets the CurrentDriveType to userDriven.
 private void SetToUserDriven()
 {
     lock (Route)
     {
         Route            = new List <mPoint>();
         Path             = new List <mPoint>();
         CurrentDriveType = DriveTypeStatus.UnRestrictedUserDriven;
     }
 }
コード例 #3
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
 public void SetRoute(mPoint nextPointUV)
 {
     lock (Route)
     {
         Route = new List <mPoint>();
         Route.Add(CurrentPose.CurrentPositionInUV);
         Route.Add(nextPointUV);
         CurrentDriveType = DriveTypeStatus.RoutDriven;
     }
 }
コード例 #4
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
 public void ToggleDoorNavigationMode()
 {
     if (CurrentDriveType != DriveTypeStatus.DoorDriven)
     {
         InitializeDoorNavigation();
         CurrentDriveType = DriveTypeStatus.DoorDriven;
     }
     else
     {
         CurrentDriveType = DriveTypeStatus.UnRestrictedUserDriven;
         StopDoorNavigation();
     }
 }
コード例 #5
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
        void DoorOrientationNavigator_OnNavigationStrategyEnded(object sender, NavigationStrategyEndedEventArgs eventArgs)
        {
            // If the door navigation wasn't able to successfully finish
            if (!eventArgs.IsComplete)
            {
                // If we encounter an error, return to user control
                CurrentDriveType = DriveTypeStatus.UserDriven;
                return;
            }

            DoorOrientationNavigator.OnNavigationStrategyEnded -= DoorOrientationNavigator_OnNavigationStrategyEnded;
            //DoorOrientationNavigator = null;
            CurrentDriveType = DriveTypeStatus.RoutDriven;
        }
コード例 #6
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
        //****************** Methods for Interface ******************

        // called by UI ******************

        public void ToggleUsermode()
        {
            //locks on route to prevent a switch in CurrentDriveType while we are updating route
            lock (Route)
            {
                if (CurrentDriveType == DriveTypeStatus.UserDriven)
                {
                    CurrentDriveType = DriveTypeStatus.UnRestrictedUserDriven;
                }
                else if (CurrentDriveType == DriveTypeStatus.UnRestrictedUserDriven)
                {
                    CurrentDriveType = DriveTypeStatus.UserDriven;
                }
            }
        }
コード例 #7
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
        public int[] MotorCalibrationRun()
        {
            //FileStream fs = new FileStream("C:\Code\Senior Design\Eris-Autonomous-Wheelchair\Logs", FileMode.);
            CurrentDriveType = DriveTypeStatus.ProgramDriven;
            Thread.Sleep(1500);
            double originalDegrees = CurrentPose.CurrentDirection;

//			U.Text = originalDegrees.ToString();
            Thread.Sleep(5000);             //time is how long wheelchair will move that way

            double endDegrees = CurrentPose.CurrentDirection;

            SetToUserDriven();
            calibrationResult = (int)(endDegrees - originalDegrees);

            int[] toRet = { (int)originalDegrees, (int)endDegrees, calibrationResult };
            return(toRet);
            //return endDegrees; //mostly debugging
        }
コード例 #8
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
        /// <summary>
        /// Signals the navigator to find a route to the given name destination.
        /// sets the driveType to driven by route unless the rout was null which
        /// means there was an error in cartographer and you cant get to that point
        /// </summary>
        /// <param name="destination">Name of location to route to.</param>
        public void NavigateTo(mPoint destinationPointInUV)
        {
            lock (Route)
            {
                mPoint currentPosition = CurrentPose.CurrentPositionInUV;

                List <mPoint> newRoute = Cartographer.PlanRoute(currentPosition, destinationPointInUV);

                //if we have a valid route set the route and switch to route driven else set to user driven
                if (newRoute != null)
                {
                    Route            = newRoute;
                    Path             = new List <mPoint>();
                    CurrentDriveType = DriveTypeStatus.RoutDriven;
                }
                else
                {
                    SetToUserDriven();
                }
            }
        }
コード例 #9
0
ファイル: Navigator.cs プロジェクト: justin-hubbard/Capstone
        //****************** Methods for Threads ******************

        /// <summary>
        /// Updates the current route, poppin the next destination off the
        /// stack if we've arrived at it. If we dont have a route to follow
        /// it will just update the current position.
        /// </summary>
        /// ToDo: might need to update to allow the chair to recover from
        /// door navigation correctly
        private void UpdateRoute()
        {
            while (true)
            {
                //update the position and grab it for use.
                mPoint mostCurrentPosition = CurrentPose.CurrentPositionInUV;

                lock (Route)
                {
                    //if we have a path to follow.
                    if (Path.Count > 0)
                    {
                        double pathDestinationDif = Path[0].GetDistanceToPoint(mostCurrentPosition);

                        //if the point are checking is a door then use the DistanceToBeAtDoor else use DistanceToBeAtPoint
                        if (Path[0].IsDoor() && pathDestinationDif < DistanceToBeAtDoor)
                        {
                            Path.RemoveAt(0);
                        }
                        else if (pathDestinationDif < DistanceToBeAtPoint)
                        {
                            Path.RemoveAt(0);
                        }
                    }

                    //Check if I'm at my next destination and if so, pop it off the stack.
                    //if we have a rout we need to follow
                    if (Route.Count > 0)
                    {
                        //get the distance from the current location to the next location
                        double distanceToDestination = Route[0].GetDistanceToPoint(mostCurrentPosition);

                        //if our destination is a door and we are there
                        if (Route[0].IsDoor() && distanceToDestination < DistanceToBeAtDoor)
                        {
                            //swap to door driven
                            InitializeDoorNavigation();
                            CurrentDriveType = DriveTypeStatus.DoorDriven;

                            Route.RemoveAt(0);
                        }
                        // if we are at the destination
                        else if (distanceToDestination < DistanceToBeAtPoint)
                        {
                            Route.RemoveAt(0);
                        }
                    }

                    //if we have a route but not a path setup the new path and we are still in routeDriven mode.
                    //Note: the RoutDriven check makes sure we are not trying to get through a door and setting up a path for it
                    if ((Route.Count > 0 && Path.Count == 0) && CurrentDriveType == DriveTypeStatus.RoutDriven)
                    {
                        List <mPoint> newPath = Cartographer.PlanPath(mostCurrentPosition, Route[0], this.ObstacleMap);

                        //if we can get to the new locating set up the path to do so else set us back to UserDriven
                        if (newPath != null)
                        {
                            Path = newPath;
                        }
                        else
                        {
                            SetToUserDriven();
                        }
                    }
                }

                Thread.Sleep(500);
            }
        }