}//end combined Check angle
//This function searched for a right turn angle which the robot can avoid an obstacle to the right.
// The function starts out by looking at the target point in, and finds a turn to the right of that angle, not to the right of straight forward.
public static double combinedRightWheelScan(LidarPoint target)
{
LocationPoint source = new LocationPoint();// source is the point at the front right corner of the right wheel
double target_angle = adjust_angle(Math.Atan2(target.X, target.Y), 2.0*Math.PI);
// this is the desired angle to the target point, //this function limits the angle between (-pi & pi) or (-180 & 180)
double sample_phi; // temporary variable which holds various angles to look at
int i = 0;// set up iterator to initailize at 0
source.X = combinedHalfRobotWidth;// assigning source point to the front right corner of the right wheel
source.Y = 0; //assigning the source point to a value of Y=0.
do// find the closest turn angle on the right of the target turn angle which will avoid obstacles
{
LocationPoint sample_point = new LocationPoint(); // create a new point which might be the point we can go towards
sample_point.X = combinedBufferLength * Math.Sin(target_angle - SAMPLING_ANGLE * i);//assign the sample point X Value
sample_point.Y = combinedBufferLength * Math.Cos(target_angle - SAMPLING_ANGLE * i);//assign sample point Y Value
sample_phi = adjust_angle(Math.Atan2(sample_point.X, sample_point.Y), 2.0 * Math.PI);//this function limits the angle between (-pi & pi) or (-180 & 180)
//check if there is anything in the way of this turn angle. Combined Vector Scan will return
//true when it is possible for the robot to take the input turn angle, and false if there is something in the way.
if (combinedVectorScan(source, sample_point))
{
//the turn angle of sample_phi is safe for this wheel
if (combinedCheckAngle(sample_phi)) //double check this turn angle is safe for both wheels
{
if (i == 0)//if this is the first itration (IE the entered desired turn angle is safe), then return 0.
{
return 0;
}
return sample_phi;//otherwise return the turn angle needed to avoid the obstacle
}//end Combined Check ANgle
}//end COmbined Vector Scan
i++;
} while (i < ANGLE_SAMPLES);
return DOOM;//if it is impossible to avoid an obstacle then return doom which is a large turn angle....
}//end Combined Right Wheel Scan
////}
/// <summary>
/// inital landmark scan correction
/// </summary>
/// <param name="CLM">landmarks found in lidar scan</param>
/// <param name="KLM">landmarks found in text file</param>
public static void CorrectLandmarks(List <LocationPoint> CLM, ref List <LidarPoint> KLM)
{
//loop through each landmark found in the list of known landmarks which were read in from a file
foreach (var knownLandmark in KLM)
{
double minMatchDist = 15;// the inital (purposefully large) distance between a KLM and a CLM. This is updated
//to the distance between the CLM and the KLM each time the distance is smaller than any previous distance.
double maxSeperationDist = .5; // the maximum distance between the KLM and CLM for a the CLM to be asscocaited with the KLM
LocationPoint closestLandmark = null; // create local temporary landmark
//look through landmarks that were found in the LiDAR scan in attempt to associate one to the list of known landmarks
foreach (var currentLandmark in CLM)
{
//calculate the distance between the current known landmark to the current found landmark
double dist_KLM__to_CLM = Math.Sqrt(Math.Pow(knownLandmark.X - currentLandmark.X, 2) + Math.Pow(knownLandmark.Y - currentLandmark.Y, 2));
//if the distance between the CLM and the KLM is smaller than the smallest distance found between the current KLM and any other CLMs
if (dist_KLM__to_CLM < minMatchDist)
{
//if the distance between the CLM and the KLM is below the seperation threshold
if (dist_KLM__to_CLM < maxSeperationDist)
{
minMatchDist = dist_KLM__to_CLM; // update the matched distance to the distance from this CLM and this KLM
closestLandmark = currentLandmark; // update the closest landmark to this CLM
}
}
}
if (closestLandmark != null) //if a CLM was found close enough to a KLM and is associated to it.
{
knownLandmark.X = closestLandmark.correctedX; //update the KLM X value
knownLandmark.Y = closestLandmark.correctedY; //Update the KLM Y value
}
else //dont update, keep landmark the same
{
}
} //end CLM loop
} //emd KLM loop
//this function checks a turn angle (provided by wheel scans) to see if there is anything in the way of the desired turn angle.
//It returns true if the robot can turn that direction without hitting an obstacle, and returns false if there is something in
//the way of the entered turn angle.
static bool combinedVectorScan(LocationPoint source, LocationPoint destination)
{
double target_angle = adjust_angle(Math.Atan2(destination.X - source.X, destination.Y - source.Y), 2.0*Math.PI); //limit angle between (-pi & pi) or (-180 & 180)
double target_dist = destination.DistanceBetween(source); //find distance between corner of wheel to desired target. this equals Buffer length
double dist;
int i = 0, j = 0;//set up two iterators
double tempDist = 0.0;
LocationPoint sample_point= new LocationPoint();// create temporary point which is updated each iteration
do// find out if there is anything in the way of the
{
sample_point.X = (i * SAMPLING_DISTANCE) * Math.Sin(target_angle);//create temporary XY point along the desired target angle at varying distances
sample_point.Y = (i * SAMPLING_DISTANCE) * Math.Cos(target_angle);//create temporary XY point along the desired target angle at varying distances
foreach (var cpoint in combinedBufPoints)//look through each point point in the buffer LiDAR points to see if any are by the temporary point
{
if (cpoint.Y < 0) //if it's behind the robot, skip it
{
continue;
}
dist = sample_point.DistanceBetween(cpoint); //figure out distance between the temporary point and this point in the buffer LiDAR points
if (dist < combinedBufferWidth) // if the dcurrent LiDAR point is close to the temporary point
{
badCount++;//increment the number of LiDAR points in the way of this target angle
if (badCount > BADCOUNTTHRESH) //if there's too many lidar points in the way then this is not a valid turn angle
{
return false;//return false to indicate that this turn angle is not good.
}//end BAD COUNT THRESH
}//END dist<combinedBufferWidth
}//end For each LiDAR point
i++;
} while (SAMPLING_DISTANCE * i < target_dist); // while the sampling distance along the target angle is less than the target distance
return true;//return true when it is possible for the robot to take this turn angle without hitting obstacles
}//end CombinedVectorScan
}//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
static int LM_POINTS_THRESH = 3; // this is the minimum number of LiDAR points needed for a landmark to be considered a landmark
/// <summary>
/// //The brains of Yeti!
//Localizatino revolves around this function entirely, which is by far the most complicated thing about yeti,
// There are really three necassary things for this to work:CLM, Robot Location, Tolerance
/// </summary>
/// <param name="CLM">list of landmarks which the robot currently sees</param>
/// <param name="robotLocation">the previously calculated position of the robot</param>
/// <param name="tolerance">the tolerance of the derivative of change in sum of error squared to be considered a local minimum</param>
/// <param name="Lspeed">not needed, but is used to determine if the robot is trying to move</param>
/// <param name="Rspeed">not needed, but is used to determine if the robot is trying to move</param>
/// <param name="minSpeed">not needed, but is used to determine if the robot is trying to move</param>
/// <returns></returns>
public static LocationPoint DetermineRobotLocation(List <LocationPoint> CLM, LocationPoint robotLocation, double tolerance, float Lspeed, float Rspeed, float minSpeed)
{
int JMAX = 15; //Maximum number of attempts to converge on a local minimum of the robots location
double mu = 0.1; //initial learning rate(used in Gradient Descent)
bool updateh = false; //flag for it the hessian matrix should be updated
double[,] H = new double[3, 3]; // Hessian of 2nd Derivatives
double[,] H_lm = new double[3, 3];
double[,] H_inv = new double[3, 3]; // Matrix Inverse of H_lm
double x_err = 0; // Gradient E [0]
double y_err = 0; // Gradient E [1]
double t_err = 0; // Gradient E [2]
double ex; // Landmark error in X
double ey; // Landmark error in Y
double dx; // Robot Delta X
double dy; // Robot Delta Y
double dt; // Robot Delta Theta
double det; // Determinant of H_lm
//initially use the last robots location as the intial guess of where the robot is.
var thisRobotLocation = new LocationPoint(robotLocation.X, robotLocation.Y, robotLocation.Heading);
double errsqrd = 0.0; // Sum of Errors Squared
double derr = 0.0; // Sum of Errors (not squared)
double lasterrsqrd = 99999.0; //inital error is set to infinity.
double lambda = 10.0; // Levenburg Marquardt step Value
/*INitialize Hessian Matrix To Zero*/
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
H[i, j] = H_lm[i, j] = H_inv[i, j] = 0;
}
}
/*Attempt to Converge on robot location*/
for (int j = 0; j < JMAX; j++)
{
errsqrd = 0; // reset the error squared for the new position which is to be calculated
derr = 0; // reset the errors derivative.
if (j == 0) // on first attempt to converge
{
updateh = true; // it is necassary to update the hessian matrix
lasterrsqrd = 99999.0; //initial error squared is set to inifinity so that first attempt at convergence is accepted
}
if (updateh) //update the hessian matrix(IE Calculate new position entirely)
{
//reset the hessian matrix to zeros
for (int i = 0; i < 3; i++)
{
for (int k = 0; k < 3; k++)
{
H[i, k] = 0.0;
}
}
//place learning rate on diagonal of the hessian
H[0, 0] = H[1, 1] = H[2, 2] = mu * CLM.Count;
//initialize error to non-zero (further away robot positions should be punished a little more heavily than previous
x_err = mu * CLM.Count * (previousRobotLocation.X - thisRobotLocation.X);
y_err = mu * CLM.Count * (previousRobotLocation.Y - thisRobotLocation.Y);
t_err = mu * CLM.Count * (previousRobotLocation.Heading - thisRobotLocation.Heading);
foreach (var currentLandmark in CLM)//loop through each landmark yeti can see
{
//figure out the cost in x and cost in y
ex = currentLandmark.X - thisRobotLocation.X -
currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading); // this is delta X for landmark i (equation 2 in ION GNSS Paper)
ey = currentLandmark.Y - thisRobotLocation.Y
- currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading); //this is delta y for landmark i(equation 3 in ION GNSS Paper)
//saving corrected landmark locations for correct landmarks
currentLandmark.correctedX = currentLandmark.X - ex; // save the correct X coordinate of the landmark for the correct landmarks function
currentLandmark.correctedY = currentLandmark.Y - ey; // save the correct Y coordinate of the landmark for the correct landmarks function.
//Note that this is for this robot location, which may be rejected.
derr += Math.Abs(ex) + Math.Abs(ey); // add the errors in both coordinates
errsqrd += Math.Pow(ex, 2) + Math.Pow(ey, 2); // add this error to the sum of error squared. (equation 1 in ION GNSS Paper)
x_err += ex; //increment the total error in the X coordinate for this robot location.
y_err += ey; //increment the total erro rin the Y coordinate for this robot location
//increment the total error in of theta
t_err += ex * (currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading)) -
ey * (currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading));
//Updating the Hessian Matrix
H[0, 0] += 1; // (equation 9 in ION GNSS Paper)
H[0, 1] += 0; // (equation 10 in ION GNSS Paper)
H[0, 2] += (currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading)); // (equation 11 in ION GNSS Paper)
H[1, 0] += 0; // (equation 12 in ION GNSS Paper)
H[1, 1] += 1; // (equation 13 in ION GNSS Paper)
H[1, 2] += -(currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading)); // (equation 14 in ION GNSS Paper)
H[2, 0] += (currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading)); // (equation 15 in ION GNSS Paper)
H[2, 1] += -(currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading)); // (equation 16 in ION GNSS Paper)
H[2, 2] += Math.Pow(currentLandmark.Distance, 2); // Formula 17
}//end for each CLM
}//end update h
// calculating the Matrix H * lamba(Diagonal Matrix). (necassary for equation 19 in ION GNSS Paper)
for (int i = 0; i < 3; i++)
{
for (int k = 0; k < 3; k++)
{
H_lm[i, k] = H[i, k];
if (i == k)
{
H_lm[i, k] += lambda * 1.0;
}
}
}
//finding the determinant , which is used to calculate the inverse of the Matrix H * lamba(Diagonal Matrix)
det = H_lm[0, 0] * (H_lm[2, 2] * H_lm[1, 1] - H_lm[2, 1] * H_lm[1, 2]) -
H_lm[1, 0] * (H_lm[2, 2] * H_lm[0, 1] - H_lm[2, 1] * H_lm[0, 2]) +
H_lm[2, 0] * (H_lm[1, 2] * H_lm[0, 1] - H_lm[1, 1] * H_lm[0, 2]);
// Find inverse of Matrix H * lamba(Diagonal Matrix). (Part of equation 19 in ION GNSS Paper)
H_inv[0, 0] = (H_lm[2, 2] * H_lm[1, 1] - H_lm[2, 1] * H_lm[1, 2]) / det;
H_inv[0, 1] = -(H_lm[2, 2] * H_lm[0, 1] - H_lm[2, 1] * H_lm[0, 2]) / det;
H_inv[0, 2] = (H_lm[1, 2] * H_lm[0, 1] - H_lm[1, 1] * H_lm[0, 2]) / det;
H_inv[1, 0] = -(H_lm[2, 2] * H_lm[1, 0] - H_lm[2, 0] * H_lm[1, 2]) / det;
H_inv[1, 1] = (H_lm[2, 2] * H_lm[0, 0] - H_lm[2, 0] * H_lm[0, 2]) / det;
H_inv[1, 2] = -(H_lm[1, 2] * H_lm[0, 0] - H_lm[1, 0] * H_lm[0, 2]) / det;
H_inv[2, 0] = (H_lm[2, 1] * H_lm[1, 0] - H_lm[2, 0] * H_lm[1, 1]) / det;
H_inv[2, 1] = -(H_lm[2, 1] * H_lm[0, 0] - H_lm[2, 0] * H_lm[0, 1]) / det;
H_inv[2, 2] = (H_lm[1, 1] * H_lm[0, 0] - H_lm[1, 0] * H_lm[0, 1]) / det;
/* Update Here*/
/* Hessian Inverse times Gradiant! (equation 19 in ION GNSS Paper)*/
// this is the change in the robot location from the intial guess used.
dx = (H_inv[0, 0] * x_err) + (H_inv[0, 1] * y_err) + (H_inv[0, 2] * t_err); // (equation 19 in ION GNSS Paper)
dy = (H_inv[1, 0] * x_err) + (H_inv[1, 1] * y_err) + (H_inv[1, 2] * t_err); // (equation 19 in ION GNSS Paper)
dt = (H_inv[2, 0] * x_err) + (H_inv[2, 1] * y_err) + (H_inv[2, 2] * t_err); // (equation 19 in ION GNSS Paper)
// if the error(not squared) is less than tolerance, meaning the position guess is almost exactly correct, so there is no need to converge further.
if (Math.Abs(derr) < tolerance)
{
j = JMAX; // break;
}
//compared this robot location guess sum of error squared to the last
if (errsqrd < lasterrsqrd)
{
//if this position has better error than the last
updateh = true; // if it does, then recalculate the hessian matrix for a new position.
lambda /= 10; // divide lambda by 10, and slow the learning rate to converge on a local minimum slower.
lasterrsqrd = errsqrd; // save the error squared so it can be compared to next iteration.
thisRobotLocation.X += dx; // (equation 19 in ION GNSS Paper)
thisRobotLocation.Y += dy; // (equation 19 in ION GNSS Paper)
thisRobotLocation.Heading += dt; // (equation 19 in ION GNSS Paper)
}
else// this position has more error than the last
{
updateh = false; //do not calculate new hessian matrix
lambda *= 10; //just increase lambda
}
}//end Convergence attempts. (J=MAX or Derivative of Error is 0).
previousRobotLocation.X = thisRobotLocation.X;
previousRobotLocation.Y = thisRobotLocation.Y;
previousRobotLocation.Heading = thisRobotLocation.Heading;
return(thisRobotLocation);
}//end determine robot location
}//end determine robot location
//Scan landmarks is used to find all of the surrounding landmark locations. To do this the LiDAR data is needed, the list of known landmark locations
//from a text file are needed, and the robots current location (to convert each point in the LiDAR data to an XY coordinate in the field).
public static List <LocationPoint> ScanLandmarks(List <LidarPoint> LMSdata, List <LidarPoint> KLM, LocationPoint robotLocation)
{
var currentLandmarks = new List <LocationPoint>(); //anytime a landmark is found in a scan the landmark is saved to this arrray
foreach (var knownLandmark in KLM) //loop through each known landmark in the list of imported landmarks
{
//smallestSeperation is the maximum distance a point can be from where a landmark should be to be considered part of the landmark
//double smallestSeparation = 0.55; ////landmark_tolerance;
double smallestSeparation = 0.45; ////landmark_tolerance;
//double smallestSeparation = 1; ////landmark_tolerance;
//double smallestSeparation = 1.75;
LocationPoint matchedLandmark = null;// create a point to match the landmark too if it is found
//The following variables must be reset to zero at the beginning of each landmark search.
double Xsum = 0, ysum = 0, headingsum = 0; //everytime a point is found to be part of a landmark the X, Y heading
// are summed together, then divded by number of points found to find the X, Y and heading averages for the points associated with the landmark
int found_points = 0; //save the number of points that become associated with the landmark
int twiceDist_points = 0; //used for debuging
//loop through every point in the LiDAR scan
foreach (var currentPoint in LMSdata)
{
if (currentPoint == null) //check if the LiDAR point did not return because there was nothing for the laser to bounce off of
{
continue; //forget about this point
}
var landmarkLocationPoint = currentPoint.ToLocationPoint(robotLocation); // convert the current point we are looking at
//into a point in the XY field based off of where the point is in the scan and where the robot is in the field.
// figure out the difference between where the landmark should be and where the landmark is in the X coordinate
double xSeparation = knownLandmark.X - landmarkLocationPoint.X;
// figure out the difference between where the landmark should be and where the landmark is in the Y coordinate
double ySeparation = knownLandmark.Y - landmarkLocationPoint.Y;
// figure out the distance between the LiDAR point we are looking at compared to where
double currentSeparation = Math.Sqrt(Math.Pow(xSeparation, 2) + Math.Pow(ySeparation, 2));
//the landmark should be
// if the seperation is beneath the set tolerance. IE the point is close enough to the landmarks expected position to be considered part
if (currentSeparation < smallestSeparation)
//of the landmark
{
Xsum += currentPoint.X; //add the point to the Xsum
ysum += currentPoint.Y; //add the point to the Ysum
headingsum += currentPoint.Heading; //add the point to the heading sum
found_points++; //increment the number of points found
}
}//end loop through LiDAR points
//now check if the points which were associated with the landmark were enough to be considered a landmark
// if the number of points is low (less than thresh), then this likely isnt a landmark and it should not be assumed that this is a landmark
if (found_points > LM_POINTS_THRESH)
{
//This is considered a landmark!
double XAverage = Xsum / (double)found_points; //find average of all X values of the points associated with the landmark
double YAverage = ysum / (double)found_points; //find average of all Y values of the points associated with the landmark
double headingAverage = headingsum / (double)found_points; //find average of all heading values of the points associated with the landmark
double distanceAverage = Math.Sqrt(XAverage * XAverage + YAverage * YAverage); //find average distance from the robot to the landmark point
if (distanceAverage < 15) ////MAX_LANDMARKS_DISTANCE)// check if the landmark is less than 15M away
{
matchedLandmark = new LocationPoint(); // we have a new landmark!
matchedLandmark.X = knownLandmark.X; //save the landmarks X value
matchedLandmark.Y = knownLandmark.Y; //save the landmarks Y value
//the next part is saving the distance between the robot and the landmark.
//40% is the distance average, and 60% is the distance from the point & the robot
//this is done because neither of the distance vaues are exactly correct due to the fact that
//the landmark locations are not exactly at the center of the landmark(as they should be)
//so it is assumed that the distance from the selected point is slightly more correct
//than the distance average(which is the sum of the distancese from all of the found points)
//40% of the distance from the landmark is the distance average
matchedLandmark.Distance = 0.4 * distanceAverage;
//60% of the distance is the distance from the found landmarks position from the robot
matchedLandmark.Distance += 0.6 * robotLocation.DistanceBetween(matchedLandmark);
matchedLandmark.Heading = headingAverage; // heading average is saved
currentLandmarks.Add(matchedLandmark); //yay we have a new landmark so add it to our list of landmarks!
continue;
}
}
}//end for each landmark loops
return(currentLandmarks);//yay here are all of our landmarks!
}
/// the function previously known as guide, returns true when the robot has reached it's waypoint.
public static bool areWeThereYetAndTurnPID(LocationPoint robot, Target currentTarget)
{
double heading = robot.Heading; // save the robots heading in the field locally
int dir = (int)currentTarget.dir; //obtain if the robot should go forward or backward (based on read in text file)
double dx, dy; //Delta X & Delta Y. Difference in X & Y coordinates the robot is from the target
//double s, c;//Sin and cosine of the robots heading
double dt; //Delta time. Holds the difference in time from the last time this function was called to this time.
double desiredAngle; // the desired heading. The heading which would cause the robot to directly face the target
bool reachedTarget; //flag to detect if the robot has made it to its waypoint
if (dir < 0) //if direction from text file says to go backward, turn heading around
{
heading = heading - Math.PI * Math.Sign(heading);
}
//FIND DISTANCE IN X & Y COORDINATES TO TARGET
dx = currentTarget.location.X - robot.X; //find difference in X coordinate from robot locaiton to target location
dy = currentTarget.location.Y - robot.Y; //find difference in X coordinate from robot locaiton to target location
//FIND DISTANCE AND ANGLE TO DESTINATION
cvar.targdist = Math.Sqrt(dx * dx + dy * dy);//current distance from the robot to the target
// desired angle is the desired Heading the robot should have at this instance if it were to be facing the target.
desiredAngle = adjust_angle(Math.Atan2(dx, dy), 2.0 * Math.PI);
//DEBUG(heading); DEBUG(desiredAngle);
//USED FOR WAYPOINT NAVIGATION
//cvar.right = dx * c - dy * s;
//cvar.front = dy * c + dx * s;
//c = Math.Cos(heading);//find Cosine term of the robots heading
//s = Math.Sin(heading);//find sine term of the robots heading
//RETRIEVE HOW FAST THE ROBOT SHOULD BE MOVING TOWARDS THIS WAYPOINT
cvar.speed = currentTarget.speed;//retrive the speed for this waypoint from navigation text file
//TIMING UPDATES FOR PID
thisTime = System.DateTime.Now.Ticks / (TICKS_PER_SECOND); //find and save the current time
dt = thisTime - lastTime; //save the difference from the last time to the current time.
/*Current Target Heading PID Calculations*/
cvar.lastpErr = cvar.pErr; //save the last proportional error
cvar.pErr = adjust_angle(heading - desiredAngle, 2.0 * Math.PI); //calculate the current propotional error betweem our current heading and the target heading
cvar.iErr = cvar.iErr + cvar.pErr * dt; // increase the cumulated error.
cvar.iErr = Math.Sign(cvar.iErr) * Math.Min(Math.Abs(cvar.iErr), maxIntErr); //limit the maxmium integral error
if (dt != 0) //if the time has changed since the last iteration of guide. (cannot divide by 0).
{
cvar.dErr = (cvar.pErr - cvar.lastpErr) / dt; // calculate the derrivative error
}
if (Math.Cos(cvar.pErr) > 0.5) // if the robot is not facing more than +-60 degrees away from the target
{
//cvar.kP = 3 / Math.Max(cvar.targdist, 3); // * 0.7112
////cvar.kP = 1;
cvar.kP = 0.5;
cvar.turn = -(cvar.kP * Math.Sin(cvar.pErr) * 2 + cvar.kI * cvar.iErr + cvar.kD * cvar.dErr); // calulate how much the robot should turn at this instant.Nattu
////cvar.turn = -(cvar.kP * cvar.pErr + cvar.kI * cvar.iErr + cvar.kD * cvar.dErr);
}
else//if the robot is facing more than 60 degrees away from the target
{
cvar.turn = -0.5 * Math.Sign(cvar.pErr); //if you need to turnin place, then ignore PID temporarily
}
lastTime = thisTime;//update the times
//inLastTarget = (lastDist < leaveTargetThresh);
//approachingTarget = (cvar.targdist < approachingThresh);
reachedTarget = (cvar.targdist < destinationThresh); //if the robot is very close to the target, then it has hit the target.
return(reachedTarget); //return true if the robot is close to the target.
}
public static List <Obstacle> FindObstacles(List <int> data, LocationPoint robot,
List <Hardware.Lidar.LidarPoint> LMSdata, List <LocationPoint> landmarks) //data is a list of lidar points
{
ClearObstacles(); // clear obstacles found
ClearState(); // reset the state machine for finding obstacles
int i = 0;
bool flag_already_linking = false;//once two points are linked this flag is set to true. And is set to false after the end of an obstacle is found.
// loop through each point of non-empty lidar data
//loop through each point which is in front of yeti, not to the side. IE only points with a positive Y value
for (int j = 360; j < data.Count - 361; j++)
{
var this_point = LMSdata[j]; //create an XY point for this distance from the LiDAR Data.
var flag_points_linked = false; //set to 1 when points are part of the same obstacle, and set to 0 at each increase of j
if (this_point.Distance < maxRadius) //if the LiDAR point is closer than the max Radius (typically 15M as this is max size of the field.)
{
for (i = 1; i < forgive_count + 1; i++) // look at the next forgive_count number of points in the lidar scan to see if they are close to this point
{
var next_point = LMSdata[j + i]; // create and XY point from the next LiDAR point
double this_dist = this_point.DistanceBetween(next_point); // find a distance between the current lidar point (i) and the next lidar point(i+j)
if (this_point.DistanceBetween(next_point) < NONSEPARATION_THRESH * i * MM2M)
// if the distance between the points is less than Nonseperation thresh * the index away from the jth lidar point(converted to meters)
{
//it has been determined that point j and point i+j are part of the same obstacle, so we need to link them togethjer
LinkPoint(j, this_point.Distance, this_point.DistanceBetween(next_point)); //link the points together!
flag_points_linked = true; // we set this flag to true to indicate that points have been linked so we can assemble a
if (!flag_already_linking) //if this Lidar point j is the first Lidar point in an obstacle
{
obj_start_ind = j; //save the starting index of the obstacles
flag_already_linking = true; //set the flag so that it is known that the lidar points are currently being linked.
}
break; // we have found a point at most forgive_count away from lidar point j, so we can stop set j = to j+i now, and keep looking at future points
}
}//end for
}
if (flag_points_linked == false) // if lidar point j+i+forgive_count through j+1 are not close enough to lidar point j to be considered the same obstalce
{
if (flag_already_linking) //the linked points from previous iterations were linking so we need to close the obstacle.
{
obj_end_ind = j; // store the last lidar point index of the obstacle
AddObstacle(j, robot, LMSdata, landmarks); // add the obstacle to the obstacle list!
}
flag_already_linking = false; //flag that the obstacle has ended
ClearState(); //reset the state of variables used in "link_points"
}
else// if lidar point j is clsoe enough to lidar point j+i, then we continue to link the points
{
j = j + i - 1; //update what j should be (used if i > 0), subtract 1 because j will be incremented at start of forloop
if (j > data.Count - 361) // if the lidar point j is behind the robot (IE the point has a negative Y value)
{
if (flag_already_linking) // if the points were being linked
{
obj_end_ind = j; // store the last lidar point index of the obstacle
AddObstacle(j, robot, LMSdata, landmarks); // add the obstacle to the obstacle list!
ClearState(); // clear the state variables used in link_points
}
}
}
}
//printObstacles(obstacles);
return(obstacles);//return the list of obstacles.
}//end find obstacles
static void AddObstacle(int z, LocationPoint robot, List <Hardware.Lidar.LidarPoint> LMSdata, List <LocationPoint> landmarks)
{
double index = z - linked_count / 2; // save the center index of the obstacle in this variable
double mag = sum / linked_count; //this finds the average distance from the robot of all the points which make up the obstacle. Saved in meters
double this_x = robot.X + mag * Math.Sin(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); //save estimated x value of the center of the obstacle in the field
double this_y = robot.Y + mag * Math.Cos(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); //save estimated Y value of the center of the obstacle in the field
//if obstacle is larger than max radius, or the number of linked points too large, or the number of linked points is too small.
if (mag > maxRadius || linked_count > highThresh || linked_count < lowThresh)
{ //this is not an obstacle.
ClearState(); //obstacle is not worth acknowledging, clear the state
}
else//This is an obscale which is not to small or big, and is less close enough to care
{
//figure out where the obstacle is in the XY field
double obj_x = robot.X + mag * Math.Sin(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save this objects x value
double obj_y = robot.Y + mag * Math.Cos(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save this objects y value
bool this_is_a_landmark_flag = false; //reset the "this is a landmark" flag
bool outside_of_field = false; // reset the "the obstacle is outside of the field" flag
//Figure out if this is a landmark or not
for (int k = 0; k < landmarks.Count(); k++)// go through each landmark position from the list of known landmarks
{
if (Math.Sqrt(Math.Pow((obj_x - landmarks[k].X), 2) + Math.Pow((obj_y - landmarks[k].Y), 2)) < is_this_landmark_thresh)
{
this_is_a_landmark_flag = true;//this is a landmark!
}
}
//figure out if the object is within the plowing field or not
if (obj_x > 4.75 || obj_x < -1.750 || obj_y > 11.75 || obj_y < -2.75)//check if obstacle is outside of Triple Ifield
//if (obj_x > 1.75 || obj_x < -1.750 || obj_y > 11.75 || obj_y < -2.75)//check if obstacle is outside of Single Ifield
{
outside_of_field = true;
}
//if the object is in the field, and is not a landmark
if (!this_is_a_landmark_flag && !outside_of_field)
{
Obstacle newObstacle = new Obstacle(); //create a new obstacle
newObstacle.X = robot.X + mag * Math.Sin(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save the obstacles X value
newObstacle.Y = robot.Y + mag * Math.Cos(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save the obstacles X value
//save start and end indexes
newObstacle.obj_start_index = obj_start_ind; // save the obstacles starting index
newObstacle.obj_end_index = obj_end_ind; //save the obstacles ending index
//convert start index into a point
newObstacle.start_point = LMSdata[obj_start_ind];
newObstacle.end_point = LMSdata[obj_end_ind];
//save characteristics of the obstacle
newObstacle.Dist = mag; // save the objects distance from the robot
newObstacle.rough_size_of_obs = obj_size_sum; // save the sum of distances of the points (EG would hold the arc length of a landmark
newObstacle.line_size_of_obs = newObstacle.start_point.DistanceBetween(newObstacle.end_point); // This is distance from start point to end point.
//if the obstacle is a straight line then the rough size and line size will be the same
if (mag < 5)// check if the obstacle is within 5 meters of the robot.
//this is done only to react to the moving obstacle if it is within 5meters.
{
//if (Math.Abs(newObstacle.line_size_of_obs - newObstacle.rough_size_of_obs) < line_size_diff_thresh)//check if the object is flat(NOt used)
//{
if (Math.Abs(newObstacle.line_size_of_obs - moving_obs_size) < moving_obs_size_thresh) // check if object is the same size as the moving obstacle.
{
newObstacle.moving = true; //we have a moving obstacle
Console.WriteLine("THIS OBSTACLE IS MOVING!"); //print that it has been seen
}
else
{
newObstacle.moving = false;
} // we know the obstacle isn't moving
}
else// if the obstacle is more than 5 meters away don't stop for it yet.
{
newObstacle.moving = false;
}
numObstacles = numObstacles + 1; //increment number of found obstacles
obstacles.Add(newObstacle); //add the new obstacle to the list of obstacles
}//end if landmark or or outside field
}//end check to add obstacle
}//end add object
public double DistanceBetween(LocationPoint p2)
{
return(Math.Sqrt((this.X - p2.X) * (this.X - p2.X) + (this.Y - p2.Y) * (this.Y - p2.Y)));
}
