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 { } }