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;
}
}
//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;
}
}
public void SetRoute(mPoint nextPointUV)
{
lock (Route)
{
Route = new List <mPoint>();
Route.Add(CurrentPose.CurrentPositionInUV);
Route.Add(nextPointUV);
CurrentDriveType = DriveTypeStatus.RoutDriven;
}
}
public void ToggleDoorNavigationMode()
{
if (CurrentDriveType != DriveTypeStatus.DoorDriven)
{
InitializeDoorNavigation();
CurrentDriveType = DriveTypeStatus.DoorDriven;
}
else
{
CurrentDriveType = DriveTypeStatus.UnRestrictedUserDriven;
StopDoorNavigation();
}
}
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;
}
//****************** 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;
}
}
}
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
}
/// <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();
}
}
}
//****************** 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);
}
}
