}//end main private static void ScanThread(object sender = null) { //READ IN NEW LIDAR DATA int?[] lidarData = lidarController.ScanData(); if (EnableGUICalls) { NewLidarData.Invoke(lidarData, EventArgs.Empty); } //print LiDAR data to GUI //create list of lidar points which actually returned a value.(This list of points will be smaller than 1081, because all points which did not return are not added to this list var lidarDataNonNull = lidarData.Select(x => x.HasValue ? x.Value : int.MaxValue).ToList(); ////OutputToFile(lidarDataNonNull);//output LiDAR data to file if you want to replay it later //LOCALIZATION var currentLandmarks = Localization.ScanLandmarks(lidarData.ToLidarPoints(19000), knownLandmarks, YetiLocation); //find the landmarks YetiLocation = Localization.DetermineRobotLocation(currentLandmarks, YetiLocation, 0.001, last_lSpeed, last_rSpeed, (float)0.01); //find where robot is based of landmarks //FIND OBSTACLEs var myObstacles = Obstacle.FindObstacles(lidarDataNonNull, YetiLocation, lidarData.ToLidar1081Points(19000), currentLandmarks); //returns a list of obstacles var movingObstacles = (from p in myObstacles where p.moving == true select true).ToArray().FirstOrDefault(); //checks if any of the found obstacles were moving. if (EnableGUICalls) { NewObstacleData.Invoke(myObstacles, EventArgs.Empty); } //print the obstacles to the GUI //SLIP Dection //check that the Queue which saves yeti location is full. Only Dequeue old location if it is full if (YetiHistory.Count > YetiHistorySize) { YetiHistory.Dequeue(); // if it is full then remove the oldest location } YetiHistory.Enqueue(new LocationPoint(YetiLocation.X, YetiLocation.Y, YetiLocation.Heading)); //add the newest location //check that the Queue which saves yeti location is full. Stall detection should only trigger if the queue is full! if (YetiHistory.Count >= YetiHistorySize) { //Find Maximum and Minimums of X, Y and Heading var minX = YetiHistory.Min(item => item.X); var maxX = YetiHistory.Max(item => item.X); var minY = YetiHistory.Min(item => item.Y); var maxY = YetiHistory.Max(item => item.Y); var minHeading = YetiHistory.Min(item => item.Heading); var maxHeading = YetiHistory.Max(item => item.Heading); // if no XY movement, no rotation, and beyond first waypoint.Beyond first way point is key!!!! //Otherwise yeti will back up at the beginning if you wait to long! if ((Math.Pow(maxX - minX, 2) + Math.Pow(maxY - minY, 2) < NoMovementTolerance) && maxHeading - minHeading < NoRotationTolerance && TargetLocation.location.id > 1 && !movingObstacles) { lastStuckTime = DateTime.Now; //save the time which slipping was detected YetiHistory.Clear(); // clear the QUeue so that stall detection cannot occur again until the queue is full if (EnableGUICalls) { StallDetected.Invoke(null, EventArgs.Empty); } //print the Stall info to the GUI } } //Correct the list of known landmarks from the text file to reflect where the landmarks are actually seen in the field if (keepCorrecting) //if this is the first LiDAR scan { Localization.CorrectLandmarks(currentLandmarks, ref knownLandmarks); //inital landmark snapshot is corrected keepCorrecting = false; // flag to signal that the landmarks have been corrected, and don't need to be corrected again. } if (EnableGUICalls) { NewLandmarkData.Invoke(currentLandmarks, EventArgs.Empty); } //print the Landmark locations to the GUI //OBSTACLE AVOIDANCE Buffer.combinedUpdatePoints(lidarData.ToLidarPoints(19000)); //Creates List of LiDAR points which have a positive Y value, and are within the Buffer distance threshold //look at angle needed to go to target waypoint, if there is an obstacle in the way, then find what turn angle is needed to avoid it to the right. double Right = Buffer.combinedRightWheelScan(TargetLocation.location.ToLidarPoint(YetiLocation)); //look at angle needed to go to target waypoint, if there is an obstacle in the way, then find what turn angle is needed to avoid it to the left. double Left = Buffer.combinedLeftWheelScan(TargetLocation.location.ToLidarPoint(YetiLocation)); //WAYPOINT NAVIGATION double turn = Control.cvar.turn; //create local variable, and initialized it to the last turn angle int dir = (int)TargetLocation.dir; //read in the direction(forward or backward) which was read in from the navigation waypoint text file //Navigate! if (Control.areWeThereYetAndTurnPID(YetiLocation, TargetLocation)) //has Yeti arrived at the target waypoint? { //true when target is reached. //reached target, change target to next waypoint from waypoint navigation list Control.initGuide(); //reset PD controller errors to 0 PrevTargetLocation = TargetLocation; //update previous target the the just reached target //assign new target from the next waypoint in the list of waypoints var newTargetLocation = TargetLocationList.SingleOrDefault(x => x.location.id == PrevTargetLocation.location.id + 1); if (newTargetLocation != null) //if it is null, we've hit the last target so don't update the target { TargetLocation = newTargetLocation; //otherwise go towards new target } } //DECIDING WHERE TO GO/TURN based off of found information //create local variables to assemble wheel speeds. double speed; float lSpeed; float rSpeed; if (movingObstacles)//if we see a movin obstacle, then stop no matter what { speed = 0; turn = 0; lSpeed = 0; rSpeed = 0; } else if (Right == 0 && Left == 0)// if there no obstacles in our way, then we are good to go and can turn to wherever waypoint navigation wants to go! { //no obsticals to avoid turn = Control.cvar.turn; //speed slower as turn steeper speed = 1 / (1 + 1 * Math.Abs(turn)) * (double)dir; speed = (double)dir * Math.Min(Math.Abs(speed), 1.0); lSpeed = (float)((speed + turnBoost * turn) * maxSpeed * Control.cvar.speed);//controlvarspeed is read in from text file, and limits speed by a percentage rSpeed = (float)((speed - turnBoost * turn) * maxSpeed * Control.cvar.speed); } else if (Right == Buffer.DOOM && Left == Buffer.DOOM) //There is no way to avoid anything to the left or the right, so back up. { lSpeed = reverseSpeed * (float)maxSpeed * (float).25; rSpeed = reverseSpeed * (float)maxSpeed * (float).25; Console.WriteLine("I reached DOOM!"); } else // there is obstacles to avoid, so the avoidance turn angle which deviates least from the waypoint is selected { if (Math.Abs(Right - turn) <= Math.Abs(Left - turn)) { //move right of obstacle turn = Right; } else if (Math.Abs(Right - turn) > Math.Abs(Left - turn)) { //move Left of obstacle turn = Left; } //speed slower as turn steeper speed = 1 / (1 + 1 * Math.Abs(turn)) * (double)dir; speed = (double)dir * Math.Min(Math.Abs(speed), 1.0); lSpeed = (float)((speed + turnBoost * .5 * turn) * maxSpeed * Control.cvar.speed); rSpeed = (float)((speed - turnBoost * .5 * turn) * maxSpeed * Control.cvar.speed); } //Send Wheel Speeds and Control Yeti //If we should still be backing up because of slip detection, then continue backing up. And double check we actually want to be moving. if (lastStuckTime.Add(reverseDuration).CompareTo(DateTime.Now) > 0 && Control.cvar.speed > 0) { motorController.SetMotorValues(reverseSpeed * (float)maxSpeed, reverseSpeed * (float)maxSpeed);//go backwards } else//otherwise send wheel speeds to yeti { motorController.SetMotorValues(lSpeed, rSpeed);//use normal values } //save previous wheel speeds. last_lSpeed = lSpeed; last_rSpeed = rSpeed; //print motor speed information to GUI if (EnableGUICalls) { double[] dataToSend = { YetiLocation.X, YetiLocation.Y, /*debug1*/ YetiLocation.Heading, Left, Right, turn, lSpeed, rSpeed, currentLandmarks.Count, Control.cvar.pErr }; NewLocationPoint.Invoke(dataToSend, EventArgs.Empty); } } //end Scan thread