private void GetDubinsPath(vec3 goal)
{
CDubins dubPath = new CDubins();
//
pivotPos = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 4),
northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 4),
heading = pivotPos.heading
};
//get the dubins path vec3 point coordinates of turn
dubinsList.Clear();
dubinsList = dubPath.GenerateDubins(pt2, goal, mf.turn);
dubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
dubList.Clear();
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
dubList.Add(pt);
}
//no longer needed
dubinsList?.Clear();
}
private void GetDubinsPath(vec3 goal)
{
CDubins dubPath = new CDubins();
//get the dubins path vec3 point coordinates of turn
dubinsList.Clear();
dubinsList = dubPath.GenerateDubins(mf.pivotAxlePos, goal);
//transfer point list to recPath class point style
dubList.Clear();
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
dubList.Add(pt);
}
}
//add the points for section, contour line points, Area Calc feature
private void AddSectionContourPathPoints()
{
{
if (recPath.isRecordOn)
{
//keep minimum speed of 1.0
double speed = pn.speed;
if (pn.speed < 1.0) speed = 1.0;
bool autoBtn = (autoBtnState == btnStates.Auto);
CRecPathPt pt = new CRecPathPt(pivotAxlePos.easting, pivotAxlePos.northing, pivotAxlePos.heading, pn.speed, autoBtn);
recPath.recList.Add(pt);
}
}
//save the north & east as previous
prevSectionPos.northing = pn.fix.northing;
prevSectionPos.easting = pn.fix.easting;
// if non zero, at least one section is on.
int sectionCounter = 0;
//send the current and previous GPS fore/aft corrected fix to each section
for (int j = 0; j < vehicle.numOfSections + 1; j++)
{
if (section[j].isSectionOn)
{
section[j].AddPathPoint(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
sectionCounter++;
}
}
//Contour Base Track.... At least One section on, turn on if not
if (sectionCounter != 0)
{
//keep the line going, everything is on for recording path
if (ct.isContourOn) ct.AddPoint();
else { ct.StartContourLine(); ct.AddPoint(); }
}
//All sections OFF so if on, turn off
else { if (ct.isContourOn) { ct.StopContourLine(); } }
//Build contour line if close enough to a patch
if (ct.isContourBtnOn) ct.BuildContourGuidanceLine(pn.fix.easting, pn.fix.northing);
}
private void GetDubinsPath(vec3 goal)
{
CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;
CDubins dubPath = new CDubins();
// current psition
pivotAxlePosRP = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotAxlePosRP.easting + (Math.Sin(pivotAxlePosRP.heading) * 3),
northing = pivotAxlePosRP.northing + (Math.Cos(pivotAxlePosRP.heading) * 3),
heading = pivotAxlePosRP.heading
};
//get the dubins path vec3 point coordinates of turn
shortestDubinsList.Clear();
shuttleDubinsList.Clear();
shortestDubinsList = dubPath.GenerateDubins(pt2, goal);
//if Dubins returns 0 elements, there is an unavoidable blockage in the way.
if (shortestDubinsList.Count > 0)
{
shortestDubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 9.0, false);
shuttleDubinsList.Add(pt);
}
return;
}
}
private void GetDubinsPath(vec3 goal)
{
//CDubins dubPath = new CDubins();
////
//pivotPos = mf.pivotAxlePos;
////bump it forward
//vec3 pt2 = new vec3
//{
// easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 4),
// northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 4),
// heading = pivotPos.heading
//};
////get the dubins path vec3 point coordinates of turn
//dubinsList.Clear();
//dubinsList = dubPath.GenerateDubins(pt2, goal);
//dubinsList.Insert(0, mf.pivotAxlePos);
////transfer point list to recPath class point style
//dubList.Clear();
//for (int i = 0; i < dubinsList.Count; i++)
//{
// CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
// dubList.Add(pt);
//}
////no longer needed
//dubinsList?.Clear();
CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.0;
CDubins dubPath = new CDubins();
// current psition
pivotPos = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 3),
northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 3),
heading = pivotPos.heading
};
//get the dubins path vec3 point coordinates of turn
dubinsList.Clear();
dubList.Clear();
dubinsList = dubPath.GenerateDubins(pt2, goal, mf.gf);
//if Dubins returns 0 elements, there is an unavoidable blockage in the way.
if (dubinsList.Count > 0)
{
dubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 9.0, false);
dubList.Add(pt);
}
return;
}
//find a path from start to goal - diagnostic, but also used later
mazeList = mf.mazeGrid.SearchForPath(pt2, goal);
//you can't get anywhere!
if (mazeList == null)
{
return;
}
//start is navigateable - maybe
int cnt = mazeList.Count;
if (cnt > 0)
{
{
int turnRadius = (int)(3 * mf.vehicle.minTurningRadius);
if (cnt > 2 * turnRadius)
{
mazeList.RemoveRange(0, turnRadius);
cnt = mazeList.Count;
mazeList.RemoveRange(cnt - turnRadius, turnRadius);
}
}
dubinsList = dubPath.GenerateDubins(pt2, mazeList[0], mf.gf);
if (dubinsList.Count > 0)
{
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 10.0, false);
dubList.Add(pt);
}
}
else
{
return; //unable to generate a dubins to the start
}
for (int i = 0; i < mazeList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(mazeList[i].easting, mazeList[i].northing, mazeList[i].heading, 15.0, false);
dubList.Add(pt);
}
dubinsList = dubPath.GenerateDubins(mazeList[mazeList.Count - 1], goal, mf.gf);
for (int i = 0; i < dubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 11.0, false);
dubList.Add(pt);
}
return;
}
}
private void PurePursuitDubins(int ptCount)
{
double dist, dx, dz;
double minDistA = 1000000, minDistB = 1000000;
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
dist = ((pivotAxlePosRP.easting - shuttleDubinsList[t].easting) * (pivotAxlePosRP.easting - shuttleDubinsList[t].easting))
+ ((pivotAxlePosRP.northing - shuttleDubinsList[t].northing) * (pivotAxlePosRP.northing - shuttleDubinsList[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
C = A; A = B; B = C;
}
//currentLocationIndex = A;
//get the distance from currently active AB line
dx = shuttleDubinsList[B].easting - shuttleDubinsList[A].easting;
dz = shuttleDubinsList[B].northing - shuttleDubinsList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//abHeading = Math.Atan2(dz, dx);
//how far from current AB Line is fix
distanceFromCurrentLinePivot = ((dz * pivotAxlePosRP.easting) - (dx * pivotAxlePosRP.northing) + (shuttleDubinsList[B].easting
* shuttleDubinsList[A].northing) - (shuttleDubinsList[B].northing * shuttleDubinsList[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//integral slider is set to 0
if (mf.vehicle.purePursuitIntegralGain != 0)
{
pivotDistanceError = distanceFromCurrentLinePivot * 0.2 + pivotDistanceError * 0.8;
if (counter2++ > 4)
{
pivotDerivative = pivotDistanceError - pivotDistanceErrorLast;
pivotDistanceErrorLast = pivotDistanceError;
counter2 = 0;
pivotDerivative *= 2;
//limit the derivative
//if (pivotDerivative > 0.03) pivotDerivative = 0.03;
//if (pivotDerivative < -0.03) pivotDerivative = -0.03;
//if (Math.Abs(pivotDerivative) < 0.01) pivotDerivative = 0;
}
//pivotErrorTotal = pivotDistanceError + pivotDerivative;
if (mf.isAutoSteerBtnOn &&
Math.Abs(pivotDerivative) < (0.1) &&
mf.avgSpeed > 2.5 &&
!mf.yt.isYouTurnTriggered)
{
//if over the line heading wrong way, rapidly decrease integral
if ((inty < 0 && distanceFromCurrentLinePivot < 0) || (inty > 0 && distanceFromCurrentLinePivot > 0))
{
inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.04;
}
else
{
if (Math.Abs(distanceFromCurrentLinePivot) > 0.02)
{
inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.02;
if (inty > 0.2)
{
inty = 0.2;
}
else if (inty < -0.2)
{
inty = -0.2;
}
}
}
}
else
{
inty *= 0.95;
}
}
else
{
inty = 0;
}
if (mf.isReverse)
{
inty = 0;
}
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivotAxlePosRP.easting - shuttleDubinsList[A].easting) * dx)
+ ((pivotAxlePosRP.northing - shuttleDubinsList[A].northing) * dz))
/ ((dx * dx) + (dz * dz));
rEastRP = shuttleDubinsList[A].easting + (U * dx);
rNorthRP = shuttleDubinsList[A].northing + (U * dz);
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();
bool ReverseHeading = !mf.isReverse;
int count = ReverseHeading ? 1 : -1;
CRecPathPt start = new CRecPathPt(rEastRP, rNorthRP, 0, 0, false);
double distSoFar = 0;
for (int i = ReverseHeading ? B : A; i < ptCount && i >= 0; i += count)
{
// used for calculating the length squared of next segment.
double tempDist = Math.Sqrt((start.easting - shuttleDubinsList[i].easting) * (start.easting - shuttleDubinsList[i].easting)
+ (start.northing - shuttleDubinsList[i].northing) * (start.northing - shuttleDubinsList[i].northing));
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
double j = (goalPointDistance - distSoFar) / tempDist; // the remainder to yet travel
goalPointRP.easting = (((1 - j) * start.easting) + (j * shuttleDubinsList[i].easting));
goalPointRP.northing = (((1 - j) * start.northing) + (j * shuttleDubinsList[i].northing));
break;
}
else
{
distSoFar += tempDist;
}
start = shuttleDubinsList[i];
}
//calc "D" the distance from pivotAxlePosRP axle to lookahead point
double goalPointDistanceSquared = glm.DistanceSquared(goalPointRP.northing, goalPointRP.easting, pivotAxlePosRP.northing, pivotAxlePosRP.easting);
//calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
//double localHeading = glm.twoPI - mf.fixHeading;
double localHeading = glm.twoPI - mf.fixHeading + inty;
ppRadiusRP = goalPointDistanceSquared / (2 * (((goalPointRP.easting - pivotAxlePosRP.easting) * Math.Cos(localHeading)) + ((goalPointRP.northing - pivotAxlePosRP.northing) * Math.Sin(localHeading))));
steerAngleRP = glm.toDegrees(Math.Atan(2 * (((goalPointRP.easting - pivotAxlePosRP.easting) * Math.Cos(localHeading))
+ ((goalPointRP.northing - pivotAxlePosRP.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));
if (steerAngleRP < -mf.vehicle.maxSteerAngle)
{
steerAngleRP = -mf.vehicle.maxSteerAngle;
}
if (steerAngleRP > mf.vehicle.maxSteerAngle)
{
steerAngleRP = mf.vehicle.maxSteerAngle;
}
if (ppRadiusRP < -500)
{
ppRadiusRP = -500;
}
if (ppRadiusRP > 500)
{
ppRadiusRP = 500;
}
radiusPointRP.easting = pivotAxlePosRP.easting + (ppRadiusRP * Math.Cos(localHeading));
radiusPointRP.northing = pivotAxlePosRP.northing + (ppRadiusRP * Math.Sin(localHeading));
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
// double angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleRP))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
//if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
//{
// steerAngleRP = glm.toDegrees(steerAngleRP > 0 ?
// (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.avgSpeed * 0.277777)))
// : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.avgSpeed * 0.277777))));
//}
//Convert to centimeters
mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero);
mf.guidanceLineSteerAngle = (short)(steerAngleRP * 100);
}
private void PurePursuitRecPath(int ptCount)
{
//find the closest 2 points to current fix
double minDistA = 9999999999;
double dist, dx, dz;
//set the search range close to current position
int top = currentPositonIndex + 5;
if (top > ptCount)
{
top = ptCount;
}
for (int t = currentPositonIndex; t < top; t++)
{
dist = ((pivotAxlePosRP.easting - recList[t].easting) * (pivotAxlePosRP.easting - recList[t].easting))
+ ((pivotAxlePosRP.northing - recList[t].northing) * (pivotAxlePosRP.northing - recList[t].northing));
if (dist < minDistA)
{
minDistA = dist;
A = t;
}
}
//Save the closest point
C = A;
//next point is the next in list
B = A + 1;
if (B == ptCount)
{
//don't go past the end of the list - "end of the line" trigger
A--;
B--;
isEndOfTheRecLine = true;
}
//save current position
currentPositonIndex = A;
//get the distance from currently active AB line
dx = recList[B].easting - recList[A].easting;
dz = recList[B].northing - recList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//how far from current AB Line is fix
distanceFromCurrentLinePivot = ((dz * pivotAxlePosRP.easting) - (dx * pivotAxlePosRP.northing) + (recList[B].easting
* recList[A].northing) - (recList[B].northing * recList[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//integral slider is set to 0
if (mf.vehicle.purePursuitIntegralGain != 0)
{
pivotDistanceError = distanceFromCurrentLinePivot * 0.2 + pivotDistanceError * 0.8;
if (counter2++ > 4)
{
pivotDerivative = pivotDistanceError - pivotDistanceErrorLast;
pivotDistanceErrorLast = pivotDistanceError;
counter2 = 0;
pivotDerivative *= 2;
//limit the derivative
//if (pivotDerivative > 0.03) pivotDerivative = 0.03;
//if (pivotDerivative < -0.03) pivotDerivative = -0.03;
//if (Math.Abs(pivotDerivative) < 0.01) pivotDerivative = 0;
}
//pivotErrorTotal = pivotDistanceError + pivotDerivative;
if (isFollowingRecPath &&
Math.Abs(pivotDerivative) < (0.1) &&
mf.avgSpeed > 2.5)
//&& Math.Abs(pivotDistanceError) < 0.2)
{
//if over the line heading wrong way, rapidly decrease integral
if ((inty < 0 && distanceFromCurrentLinePivot < 0) || (inty > 0 && distanceFromCurrentLinePivot > 0))
{
inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.04;
}
else
{
if (Math.Abs(distanceFromCurrentLinePivot) > 0.02)
{
inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.02;
if (inty > 0.2)
{
inty = 0.2;
}
else if (inty < -0.2)
{
inty = -0.2;
}
}
}
}
else
{
inty *= 0.95;
}
}
else
{
inty = 0;
}
if (mf.isReverse)
{
inty = 0;
}
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivotAxlePosRP.easting - recList[A].easting) * dx)
+ ((pivotAxlePosRP.northing - recList[A].northing) * dz))
/ ((dx * dx) + (dz * dz));
rEastRP = recList[A].easting + (U * dx);
rNorthRP = recList[A].northing + (U * dz);
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();
bool ReverseHeading = !mf.isReverse;
int count = ReverseHeading ? 1 : -1;
CRecPathPt start = new CRecPathPt(rEastRP, rNorthRP, 0, 0, false);
double distSoFar = 0;
for (int i = ReverseHeading ? B : A; i < ptCount && i >= 0; i += count)
{
// used for calculating the length squared of next segment.
double tempDist = Math.Sqrt((start.easting - recList[i].easting) * (start.easting - recList[i].easting)
+ (start.northing - recList[i].northing) * (start.northing - recList[i].northing));
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
double j = (goalPointDistance - distSoFar) / tempDist; // the remainder to yet travel
goalPointRP.easting = (((1 - j) * start.easting) + (j * recList[i].easting));
goalPointRP.northing = (((1 - j) * start.northing) + (j * recList[i].northing));
break;
}
else
{
distSoFar += tempDist;
}
start = recList[i];
}
//calc "D" the distance from pivotAxlePosRP axle to lookahead point
double goalPointDistanceSquared = glm.DistanceSquared(goalPointRP.northing, goalPointRP.easting, pivotAxlePosRP.northing, pivotAxlePosRP.easting);
//calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading + inty;
ppRadiusRP = goalPointDistanceSquared / (2 * (((goalPointRP.easting - pivotAxlePosRP.easting) * Math.Cos(localHeading)) + ((goalPointRP.northing - pivotAxlePosRP.northing) * Math.Sin(localHeading))));
steerAngleRP = glm.toDegrees(Math.Atan(2 * (((goalPointRP.easting - pivotAxlePosRP.easting) * Math.Cos(localHeading))
+ ((goalPointRP.northing - pivotAxlePosRP.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));
if (steerAngleRP < -mf.vehicle.maxSteerAngle)
{
steerAngleRP = -mf.vehicle.maxSteerAngle;
}
if (steerAngleRP > mf.vehicle.maxSteerAngle)
{
steerAngleRP = mf.vehicle.maxSteerAngle;
}
//Convert to centimeters
mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero);
mf.guidanceLineSteerAngle = (short)(steerAngleRP * 100);
}
private void GetDubinsPath(vec3 goal)
{
CDubins.turningRadius = mf.vehicle.minTurningRadius * 2.0;
CDubins dubPath = new CDubins();
// current psition
pivotAxlePosRP = mf.pivotAxlePos;
//bump it forward
vec3 pt2 = new vec3
{
easting = pivotAxlePosRP.easting + (Math.Sin(pivotAxlePosRP.heading) * 3),
northing = pivotAxlePosRP.northing + (Math.Cos(pivotAxlePosRP.heading) * 3),
heading = pivotAxlePosRP.heading
};
//get the dubins path vec3 point coordinates of turn
shortestDubinsList.Clear();
shuttleDubinsList.Clear();
shortestDubinsList = dubPath.GenerateDubins(pt2, goal, mf.gf);
//if Dubins returns 0 elements, there is an unavoidable blockage in the way.
if (shortestDubinsList.Count > 0)
{
shortestDubinsList.Insert(0, mf.pivotAxlePos);
//transfer point list to recPath class point style
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 9.0, false);
shuttleDubinsList.Add(pt);
}
return;
}
//else
//{
// shuttleDubinsList?.Clear();
// return;
//}
//find a path from start to goal - diagnostic, but also used later
mazeList = mf.mazeGrid.SearchForPath(pt2, goal);
//you can't get anywhere!
if (mazeList == null)
{
return;
}
//start is navigateable - maybe
if (mazeList.Count > 0)
{
shortestDubinsList = dubPath.GenerateDubins(pt2, mazeList[0], mf.gf);
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 10.0, false);
shuttleDubinsList.Add(pt);
}
for (int i = 0; i < mazeList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(mazeList[i].easting, mazeList[i].northing, mazeList[i].heading, 15.0, false);
shuttleDubinsList.Add(pt);
}
shortestDubinsList = dubPath.GenerateDubins(mazeList[mazeList.Count - 1], goal, mf.gf);
for (int i = 0; i < shortestDubinsList.Count; i++)
{
CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 11.0, false);
shuttleDubinsList.Add(pt);
}
return;
}
else
{
}
}
