private void Bot_ObstaclesDetected(object sender, ObstaclesDetectedArgs args)
{
_obstacles = args.Obstacles;
_currentPosition = args.CurrentPosition;
_currentOrientation = args.CurrentOrientation;
var rover = (DuckieBot)sender;
Application.Current.Dispatcher.Invoke(() => {
DrawMap(_map);
});
}
// CHANGED TODO:
// This function
void ObstacleDetector_ObstaclesDetected(object sender, ObstaclesDetectedArgs e)
{
int i = 0;
setCurrentDriveType(1); // set to user driven
foreach (Obstacle obstacle in e.Obstacles) // loop through the list for the detected obstacles event
{
if (obstacle.rightPoint.X > 0 || obstacle.leftPoint.X > 0) // if the right/left X point is +
{
int right, left, diff;
right = (int)obstacle.rightPoint.X; // right X point of the obstacle
left = (int)obstacle.leftPoint.X; // left X point of the obstacle
diff = right - left; // difference of X values
//List<mPoint> points = this.ObstacleLocalizer.LocalizeObstacle(obstacle); // OLD
//this.ObstacleMap.AddObstaclePoints(points, CurrentPose.CurrentRoom); // OLD
/*Console.WriteLine("OBSTACLE DETECTED: {0}", i); // Write the number of objects detected
* Console.WriteLine("Right: {0}, Left: {1} \nDiff: {2}", right, left, diff);
* Console.WriteLine("Right: ({0},{1}) \nLeft: ({2},{3})",
* obstacle.rightPoint.X, obstacle.rightPoint.Y, obstacle.leftPoint.X, obstacle.leftPoint.Y);*/
double depth = (obstacle.rightPoint.Y + obstacle.leftPoint.Y) / 2; // set average of Y points for depth
//Console.WriteLine("Depth: {0}\n", depth);
//if ((obstacle.rightPoint.X - obstacle.leftPoint.X > 350) && obstacle.rightPoint.Y < 600)
/*if (depth < 600.0)
* setCurrentDriveType(5); // stop the motor...
* else
* setCurrentDriveType(1); // set to user driven
*/
/*
* if (i > 1) // possbly change to == 1
* {
* //setCurrentDriveType(4); // stop the motor when an object is detected
* Console.WriteLine("BRAKED!");
* Thread.Sleep(1000); // Possibly delete or change. // Lets the thread sleep...
* setCurrentDriveType(1); // Set the drive type back to user driven
*
* break;
* }
*/
}
} // end of for each loop for obstacle events
} // end of ObstacleDetector_ObstaclesDetected function
void obstacleDetector_ObstaclesDetected(object sender, ObstaclesDetectedArgs e)
{
_detectedObstacles = e.Obstacles;
}
