public bool FindHeadlandDistance() { if (headArr[0].hdLine.Count == 0) { leftToolDistance = 99999; rightToolDistance = 99999; return(false); } else { leftToolDistance = 999999; rightToolDistance = 999999; double minDistA = 1000000, minDistB = 1000000; int ptCount = headArr[0].hdLine.Count; //find the closest 2 points to current fix for (int t = 0; t < ptCount; t++) { double dist = glm.DistanceSquared(mf.section[0].leftPoint, headArr[0].hdLine[t]); 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) { Q = A; A = B; B = Q; } double dx = headArr[0].hdLine[B].easting - headArr[0].hdLine[A].easting; double dz = headArr[0].hdLine[B].northing - headArr[0].hdLine[A].northing; if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon) { return(false); } //abHeading = Math.Atan2(dz, dx); //double abHeading = curList[A].heading; //how far from current AB Line is fix leftToolDistance = ((dz * mf.section[0].leftPoint.easting) - (dx * mf.section[0].leftPoint.northing) + (headArr[0].hdLine[B].easting * headArr[0].hdLine[A].northing) - (headArr[0].hdLine[B].northing * headArr[0].hdLine[A].easting)) / Math.Sqrt((dz * dz) + (dx * dx)); //are we on the right side or not //bool isOnRightSideCurrentLine = leftToolDistance > 0; //absolute the distance //leftToolDistance = Math.Abs(leftToolDistance); //find the closest 2 points to current fixm for right side minDistA = 1000000; minDistB = 1000000; for (int t = 0; t < ptCount; t++) { double dist = glm.DistanceSquared(mf.section[mf.tool.numOfSections - 1].rightPoint, headArr[0].hdLine[t]); if (dist < minDistA) { minDistB = minDistA; D = C; minDistA = dist; C = t; } else if (dist < minDistB) { minDistB = dist; D = t; } } //just need to make sure the points continue ascending or heading switches all over the place if (C > D) { Q = C; C = D; D = Q; } dx = headArr[0].hdLine[D].easting - headArr[0].hdLine[C].easting; dz = headArr[0].hdLine[D].northing - headArr[0].hdLine[C].northing; if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon) { return(false); } //abHeading = Math.Atan2(dz, dx); //double abHeading = curList[C].heading; //how far from current AB Line is fix rightToolDistance = ((dz * mf.section[mf.tool.numOfSections - 1].rightPoint.easting) - (dx * mf.section[mf.tool.numOfSections - 1].rightPoint.northing) + (headArr[0].hdLine[D].easting * headArr[0].hdLine[C].northing) - (headArr[0].hdLine[D].northing * headArr[0].hdLine[C].easting)) / Math.Sqrt((dz * dz) + (dx * dx)); //are we on the right side or not //bool isOnRightSideCurrentLine = leftToolDistance > 0; //absolute the distance //leftToolDistance = Math.Abs(leftToolDistance); //double scanWidthR = (mf.tool.toolWidth * 0.75); if (leftToolDistance > 0 && rightToolDistance > 0) { isToolInWorkArea = true; isToolInBothPlaces = isToolInHeadland = false; } else if (leftToolDistance < 0 && rightToolDistance < 0) { isToolInHeadland = true; isToolInBothPlaces = isToolInWorkArea = false; } else { isToolInBothPlaces = true; isToolInHeadland = isToolInWorkArea = false; } vec3 toolFix = mf.toolPos; double headAB = toolFix.heading; downL.easting = mf.section[0].leftPoint.easting + (Math.Sin(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarLeftSpeed); downL.northing = mf.section[0].leftPoint.northing + (Math.Cos(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarLeftSpeed); downR.easting = mf.section[mf.tool.numOfSections - 1].rightPoint.easting + (Math.Sin(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarRightSpeed); downR.northing = mf.section[mf.tool.numOfSections - 1].rightPoint.northing + (Math.Cos(headAB) * mf.vehicle.hydLiftLookAhead * mf.tool.toolFarRightSpeed); //bool isToolFullyOut = bool isLookRightIn = IsPointInsideHeadLine(downR); bool isLookLeftIn = IsPointInsideHeadLine(downL); if (isLookLeftIn && isLookRightIn) { isLookInWorkArea = true; isLookInBothPlaces = isLookInHealand = false; } else if (!isLookLeftIn && !isLookRightIn) { isLookInHealand = true; isLookInBothPlaces = isLookInWorkArea = false; } else { isLookInBothPlaces = true; isLookInHealand = isLookInWorkArea = false; } //isToolUp = false; if (!isLookInHealand && isToolUp) { isToolUp = false; } if (isLookInHealand && isToolInHeadland && !isToolUp) { isToolUp = true; } if (isToolUp) { mf.mc.pgn[mf.mc.azRelayData][mf.mc.rdHydLift] = 1; } else { mf.mc.pgn[mf.mc.azRelayData][mf.mc.rdHydLift] = 0; } return(isToolUp); } }
private void btnLoadBoundaryFromGE_Click(object sender, EventArgs e) { if (sender is Button button) { Selectedreset = true; string fileAndDirectory; { //create the dialog instance OpenFileDialog ofd = new OpenFileDialog { //set the filter to text KML only Filter = "KML files (*.KML)|*.KML", //the initial directory, fields, for the open dialog InitialDirectory = mf.fieldsDirectory + mf.currentFieldDirectory }; //was a file selected if (ofd.ShowDialog() == DialogResult.Cancel) { return; } else { fileAndDirectory = ofd.FileName; } } string coordinates = null; int startIndex; int i = 0; using (StreamReader reader = new StreamReader(fileAndDirectory)) { if (button.Name == "btnLoadMultiBoundaryFromGE") { ResetAllBoundary(); } else { i = mf.bnd.boundarySelected; } try { while (!reader.EndOfStream) { //start to read the file string line = reader.ReadLine(); startIndex = line.IndexOf("<coordinates>"); if (startIndex != -1) { while (true) { int endIndex = line.IndexOf("</coordinates>"); if (endIndex == -1) { //just add the line if (startIndex == -1) { coordinates += line.Substring(0); } else { coordinates += line.Substring(startIndex + 13); } } else { if (startIndex == -1) { coordinates += line.Substring(0, endIndex); } else { coordinates += line.Substring(startIndex + 13, endIndex - (startIndex + 13)); } break; } line = reader.ReadLine(); line = line.Trim(); startIndex = -1; } line = coordinates; char[] delimiterChars = { ' ', '\t', '\r', '\n' }; string[] numberSets = line.Split(delimiterChars); //at least 3 points if (numberSets.Length > 2) { mf.bnd.bndArr.Add(new CBoundaryLines()); mf.turn.turnArr.Add(new CTurnLines()); foreach (var item in numberSets) { string[] fix = item.Split(','); double.TryParse(fix[0], NumberStyles.Float, CultureInfo.InvariantCulture, out lonK); double.TryParse(fix[1], NumberStyles.Float, CultureInfo.InvariantCulture, out latK); mf.pn.ConvertWGS84ToLocal(latK, lonK, out norting, out easting); //add the point to boundary vec3 bndPt = new vec3(easting, norting, 0); mf.bnd.bndArr[i].bndLine.Add(bndPt); } //build the boundary, make sure is clockwise for outer counter clockwise for inner bool isCW = mf.bnd.bndArr[i].CalculateBoundaryArea(); if (mf.bnd.boundarySelected == 0 && isCW) { mf.bnd.bndArr[i].ReverseWinding(); } //inner boundaries if (mf.bnd.boundarySelected > 0 && !isCW) { mf.bnd.bndArr[i].ReverseWinding(); } mf.bnd.bndArr[i].FixBoundaryLine(i); mf.bnd.bndArr[i].PreCalcBoundaryEarLines(); mf.bnd.bndArr[i].PreCalcBoundaryLines(); mf.bnd.bndArr[i].isSet = true; mf.fd.UpdateFieldBoundaryGUIAreas(); mf.btnMakeLinesFromBoundary.Visible = true; coordinates = ""; i++; } else { mf.TimedMessageBox(2000, gStr.gsErrorreadingKML, gStr.gsChooseBuildDifferentone); } if (button.Name == "btnLoadBoundaryFromGE") { break; } } } mf.FileSaveBoundary(); mf.turn.BuildTurnLines(); mf.btnMakeLinesFromBoundary.Visible = true; mf.fd.UpdateFieldBoundaryGUIAreas(); UpdateChart(); } catch (Exception) { return; } } } mf.bnd.isOkToAddPoints = false; panelMain.Visible = true; panelChoose.Visible = false; panelKML.Visible = false; this.Size = new Size(566, 377); UpdateChart(); }
public void BuildGeoFenceLines() { //update the GUI values for boundaries //mf.fd.UpdateFieldBoundaryGUIAreas(); if (mf.bnd.bndArr.Count == 0) { //mf.TimedMessageBox(1500, " No Boundary ", "No GeoFence Made"); return; } //to fill the list of line points vec3 point = new vec3(); //determine how wide a headland space double totalHeadWidth = mf.yt.geoFenceDistance; //outside boundary - count the points from the boundary geoFenceArr[0].geoFenceLine.Clear(); int ptCount = mf.bnd.bndArr[0].bndLine.Count; for (int i = ptCount - 1; i >= 0; i--) { //calculate the point inside the boundary point.easting = mf.bnd.bndArr[0].bndLine[i].easting + (-Math.Sin(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * totalHeadWidth); point.northing = mf.bnd.bndArr[0].bndLine[i].northing + (-Math.Cos(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * totalHeadWidth); point.heading = mf.bnd.bndArr[0].bndLine[i].heading; if (point.heading < -glm.twoPI) { point.heading += glm.twoPI; } //only add if inside actual field boundary if (mf.bnd.bndArr[0].IsPointInsideBoundary(point)) { vec2 tPnt = new vec2(point.easting, point.northing); geoFenceArr[0].geoFenceLine.Add(tPnt); } } geoFenceArr[0].FixGeoFenceLine(totalHeadWidth, mf.bnd.bndArr[0].bndLine, mf.tool.toolWidth); geoFenceArr[0].PreCalcTurnLines(); //inside boundaries for (int j = 1; j < mf.bnd.bndArr.Count; j++) { geoFenceArr[j].geoFenceLine.Clear(); if (!mf.bnd.bndArr[j].isSet || mf.bnd.bndArr[j].isDriveThru) { continue; } ptCount = mf.bnd.bndArr[j].bndLine.Count; for (int i = ptCount - 1; i >= 0; i--) { //calculate the point outside the boundary point.easting = mf.bnd.bndArr[j].bndLine[i].easting + (-Math.Sin(glm.PIBy2 + mf.bnd.bndArr[j].bndLine[i].heading) * totalHeadWidth); point.northing = mf.bnd.bndArr[j].bndLine[i].northing + (-Math.Cos(glm.PIBy2 + mf.bnd.bndArr[j].bndLine[i].heading) * totalHeadWidth); point.heading = mf.bnd.bndArr[j].bndLine[i].heading; if (point.heading < -glm.twoPI) { point.heading += glm.twoPI; } //only add if outside actual field boundary if (!mf.bnd.bndArr[j].IsPointInsideBoundary(point)) { vec2 tPnt = new vec2(point.easting, point.northing); geoFenceArr[j].geoFenceLine.Add(tPnt); } } geoFenceArr[j].FixGeoFenceLine(totalHeadWidth, mf.bnd.bndArr[j].bndLine, mf.tool.toolWidth * 0.5); geoFenceArr[j].PreCalcTurnLines(); } //mf.TimedMessageBox(800, "Turn Lines", "Turn limits Created"); }
private void oglSelf_MouseDown(object sender, MouseEventArgs e) { btnCancelTouch.Enabled = true; btnMakeABLine.Enabled = false; btnMakeCurve.Enabled = false; isMakingAB = isMakingCurve = false; Point pt = oglSelf.PointToClient(Cursor.Position); //Convert to Origin in the center of window, 800 pixels fixPt.X = pt.X - 350; fixPt.Y = (700 - pt.Y - 350); vec3 plotPt = new vec3 { //convert screen coordinates to field coordinates easting = fixPt.X * mf.maxFieldDistance / 632.0, northing = fixPt.Y * mf.maxFieldDistance / 632.0, heading = 0 }; plotPt.easting += mf.fieldCenterX; plotPt.northing += mf.fieldCenterY; pint.easting = plotPt.easting; pint.northing = plotPt.northing; if (isA) { double minDistA = 1000000, minDistB = 1000000; start = 99999; end = 99999; int ptCount = arr.Length; if (ptCount > 0) { //find the closest 2 points to current fix for (int t = 0; t < ptCount; t++) { double dist = ((pint.easting - arr[t].easting) * (pint.easting - arr[t].easting)) + ((pint.northing - arr[t].northing) * (pint.northing - arr[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) { E = A; A = B; B = E; } start = A; } isA = false; } else { double minDistA = 1000000, minDistB = 1000000; int ptCount = arr.Length; if (ptCount > 0) { //find the closest 2 points to current point for (int t = 0; t < ptCount; t++) { double dist = ((pint.easting - arr[t].easting) * (pint.easting - arr[t].easting)) + ((pint.northing - arr[t].northing) * (pint.northing - arr[t].northing)); if (dist < minDistA) { minDistB = minDistA; D = C; minDistA = dist; C = t; } else if (dist < minDistB) { minDistB = dist; D = t; } } //just need to make sure the points continue ascending or heading switches all over the place if (C > D) { E = C; C = D; D = E; } } isA = true; int A1 = Math.Abs(A - C); int B1 = Math.Abs(A - D); int C1 = Math.Abs(B - C); int D1 = Math.Abs(B - D); if (A1 <= B1 && A1 <= C1 && A1 <= D1) { start = A; end = C; } else if (B1 <= A1 && B1 <= C1 && B1 <= D1) { start = A; end = D; } else if (C1 <= B1 && C1 <= A1 && C1 <= D1) { start = B; end = C; } else if (D1 <= B1 && D1 <= C1 && D1 <= A1) { start = B; end = D; } if (start > end) { E = start; start = end; end = E; } btnMakeABLine.Enabled = true; btnMakeCurve.Enabled = true; } }
private void BtnMakeCurve_Click(object sender, EventArgs e) { btnCancelTouch.Enabled = false; double moveDist = (double)nudDistance.Value * mf.inchOrCm2m; double distSq = (moveDist) * (moveDist) * 0.999; mf.curve.refList?.Clear(); vec3 pt3 = new vec3(arr[start]); for (int i = start; i < end; i++) { //calculate the point inside the boundary pt3.easting = arr[i].easting - (Math.Sin(glm.PIBy2 + arr[i].heading) * (moveDist)); pt3.northing = arr[i].northing - (Math.Cos(glm.PIBy2 + arr[i].heading) * (moveDist)); pt3.heading = arr[i].heading; bool Add = true; for (int j = start; j < end; j++) { double check = glm.DistanceSquared(pt3.northing, pt3.easting, arr[j].northing, arr[j].easting); if (check < distSq) { Add = false; break; } } if (Add) { if (mf.curve.refList.Count > 0) { double dist = ((pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting) * (pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting)) + ((pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing) * (pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing)); if (dist > 1) { mf.curve.refList.Add(pt3); } } else { mf.curve.refList.Add(pt3); } } } int cnt = mf.curve.refList.Count; if (cnt > 3) { //make sure distance isn't too big between points on Turn for (int i = 0; i < cnt - 1; i++) { int j = i + 1; //if (j == cnt) j = 0; double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]); if (distance > 1.6) { vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0, (mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0, mf.curve.refList[i].heading); mf.curve.refList.Insert(j, pointB); cnt = mf.curve.refList.Count; i = -1; } } //who knows which way it actually goes mf.curve.CalculateTurnHeadings(); //calculate average heading of line double x = 0, y = 0; mf.curve.isCurveSet = true; foreach (vec3 pt in mf.curve.refList) { x += Math.Cos(pt.heading); y += Math.Sin(pt.heading); } x /= mf.curve.refList.Count; y /= mf.curve.refList.Count; mf.curve.aveLineHeading = Math.Atan2(y, x); if (mf.curve.aveLineHeading < 0) { mf.curve.aveLineHeading += glm.twoPI; } //build the tail extensions mf.curve.AddFirstLastPoints(); mf.curve.SmoothAB(4); mf.curve.CalculateTurnHeadings(); mf.curve.isCurveSet = true; mf.curve.curveArr.Add(new CCurveLines()); mf.curve.numCurveLines = mf.curve.curveArr.Count; mf.curve.numCurveLineSelected = mf.curve.numCurveLines; //array number is 1 less since it starts at zero int idx = mf.curve.curveArr.Count - 1; //create a name mf.curve.curveArr[idx].Name = (Math.Round(glm.toDegrees(mf.curve.aveLineHeading), 1)).ToString(CultureInfo.InvariantCulture) + "\u00B0" + mf.FindDirection(mf.curve.aveLineHeading) + DateTime.Now.ToString("hh:mm:ss", CultureInfo.InvariantCulture); mf.curve.curveArr[idx].aveHeading = mf.curve.aveLineHeading; //write out the Curve Points foreach (vec3 item in mf.curve.refList) { mf.curve.curveArr[idx].curvePts.Add(item); } mf.FileSaveCurveLines(); //update the arrays btnMakeABLine.Enabled = false; btnMakeCurve.Enabled = false; isMakingCurve = false; isMakingAB = false; start = 99999; end = 99999; FixLabelsCurve(); } else { mf.curve.isCurveSet = false; mf.curve.refList?.Clear(); } btnExit.Focus(); }
public void UpdatePosition() { if (isFollowingDubinsToPath) { //set a speed of 7 kmh mf.sim.stepDistance = 7 / 17.86; pivotAxlePosRP = mf.pivotAxlePos; FindGoalPointDubinsPath(dubListCount); PurePursuit(); //check if close to recorded path double distSqr = glm.DistanceSquared(pivotAxlePosRP.northing, pivotAxlePosRP.easting, recList[0].northing, recList[0].easting); if (distSqr < 2) { isFollowingRecPath = true; isFollowingDubinsToPath = false; dubList.Clear(); dubinsList.Clear(); } } if (isFollowingRecPath) { pivotAxlePosRP = mf.pivotAxlePos; FindGoalPointRecPath(recListCount); PurePursuit(); //if end of the line then stop if (!isEndOfTheRecLine) { mf.sim.stepDistance = recList[C].speed / 17.86; north = recList[C].northing; //section control - only if different click the button bool autoBtn = (mf.autoBtnState == FormGPS.btnStates.Auto); trig = autoBtn; if (autoBtn != recList[C].autoBtnState) { mf.btnSectionOffAutoOn.PerformClick(); } } else { //create the dubins path based on start and goal to start of recorded path GetDubinsPath(homePos); dubListCount = dubList.Count; //its too small if (dubListCount < 5) { StopDrivingRecordedPath(); } //set all the flags isFollowingDubinsHome = true; isFollowingRecPath = false; isFollowingDubinsToPath = false; isEndOfTheRecLine = false; } } if (isFollowingDubinsHome) { mf.sim.stepDistance = 7 / 17.86; pivotAxlePosRP = mf.pivotAxlePos; FindGoalPointDubinsPath(dubListCount); PurePursuit(); //check if close to home position double distSqr = glm.DistanceSquared(pivotAxlePosRP.easting, pivotAxlePosRP.northing, homePos.easting, homePos.northing); if (distSqr < 3) { StopDrivingRecordedPath(); } } //if paused, set the sim to 0 if (isPausedDrivingRecordedPath) { mf.sim.stepDistance = 0 / 17.86; } }
public vec3(vec3 v) { easting = v.easting; northing = v.northing; heading = v.heading; }
public void CreateBndTramRef() { //count the points from the boundary int ptCount = mf.bnd.bndArr[0].bndLine.Count; outArr?.Clear(); //outside point vec3 pt3 = new vec3(); double distSq = ((tramWidth * 0.5) - halfWheelTrack) * ((tramWidth * 0.5) - halfWheelTrack) * 0.97; bool fail = false; //make the boundary tram outer array for (int i = 0; i < ptCount; i++) { //calculate the point inside the boundary pt3.easting = mf.bnd.bndArr[0].bndLine[i].easting - (Math.Sin(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * (tramWidth * 0.5 - halfWheelTrack)); pt3.northing = mf.bnd.bndArr[0].bndLine[i].northing - (Math.Cos(glm.PIBy2 + mf.bnd.bndArr[0].bndLine[i].heading) * (tramWidth * 0.5 - halfWheelTrack)); for (int j = 0; j < ptCount; j++) { double check = glm.DistanceSquared(pt3.northing, pt3.easting, mf.bnd.bndArr[0].bndLine[j].northing, mf.bnd.bndArr[0].bndLine[j].easting); if (check < distSq) { fail = true; break; } } if (!fail) { pt3.heading = mf.bnd.bndArr[0].bndLine[i].heading; outArr.Add(pt3); } fail = false; } int cnt = outArr.Count; if (cnt < 6) { return; } const double spacing = 2.0; double distance; for (int i = 0; i < cnt - 1; i++) { distance = glm.Distance(outArr[i], outArr[i + 1]); if (distance < spacing) { outArr.RemoveAt(i + 1); cnt = outArr.Count; i--; } } }
public static double Distance(vec3 first, vec2 second) { return(Math.Sqrt( Math.Pow(first.easting - second.easting, 2) + Math.Pow(first.northing - second.northing, 2))); }
public void GetCurrentCurveLine(vec3 pivot, vec3 steer) { int ptCount = refList.Count; int ptCnt = ptCount - 1; if (ptCount < 5) { return; } boxA.easting = pivot.easting - (Math.Sin(aveLineHeading + glm.PIBy2) * 2000); boxA.northing = pivot.northing - (Math.Cos(aveLineHeading + glm.PIBy2) * 2000); boxB.easting = pivot.easting + (Math.Sin(aveLineHeading + glm.PIBy2) * 2000); boxB.northing = pivot.northing + (Math.Cos(aveLineHeading + glm.PIBy2) * 2000); boxC.easting = boxB.easting + (Math.Sin(aveLineHeading) * 1.0); boxC.northing = boxB.northing + (Math.Cos(aveLineHeading) * 1.0); boxD.easting = boxA.easting + (Math.Sin(aveLineHeading) * 1.0); boxD.northing = boxA.northing + (Math.Cos(aveLineHeading) * 1.0); boxA.easting -= (Math.Sin(aveLineHeading) * 1.0); boxA.northing -= (Math.Cos(aveLineHeading) * 1.0); boxB.easting -= (Math.Sin(aveLineHeading) * 1.0); boxB.northing -= (Math.Cos(aveLineHeading) * 1.0); //determine if point are in frustum box for (int s = 0; s < ptCnt; s++) { if ((((boxB.easting - boxA.easting) * (refList[s].northing - boxA.northing)) - ((boxB.northing - boxA.northing) * (refList[s].easting - boxA.easting))) < 0) { continue; } if ((((boxD.easting - boxC.easting) * (refList[s].northing - boxC.northing)) - ((boxD.northing - boxC.northing) * (refList[s].easting - boxC.easting))) < 0) { continue; } closestRefIndex = s; break; } double dist = ((pivot.easting - refList[closestRefIndex].easting) * (pivot.easting - refList[closestRefIndex].easting)) + ((pivot.northing - refList[closestRefIndex].northing) * (pivot.northing - refList[closestRefIndex].northing)); //determine closest point double minDistance = Math.Sqrt(dist); //grab the heading at the closest point refHeading = refList[closestRefIndex].heading; //which side of the patch are we on is next //calculate endpoints of reference line based on closest point refPoint1.easting = refList[closestRefIndex].easting - (Math.Sin(refHeading) * 50.0); refPoint1.northing = refList[closestRefIndex].northing - (Math.Cos(refHeading) * 50.0); refPoint2.easting = refList[closestRefIndex].easting + (Math.Sin(refHeading) * 50.0); refPoint2.northing = refList[closestRefIndex].northing + (Math.Cos(refHeading) * 50.0); //x2-x1 double dx = refPoint2.easting - refPoint1.easting; //z2-z1 double dz = refPoint2.northing - refPoint1.northing; //how far are we away from the reference line at 90 degrees - 2D cross product and distance double distanceFromRefLine = ((dz * pivot.easting) - (dx * pivot.northing) + (refPoint2.easting * refPoint1.northing) - (refPoint2.northing * refPoint1.easting)); // / Math.Sqrt((dz * dz) + (dx * dx)); //are we going same direction as stripList was created? isSameWay = Math.PI - Math.Abs(Math.Abs(pivot.heading - refHeading) - Math.PI) < glm.PIBy2; deltaOfRefAndAveHeadings = Math.PI - Math.Abs(Math.Abs(aveLineHeading - refHeading) - Math.PI); deltaOfRefAndAveHeadings = Math.Cos(deltaOfRefAndAveHeadings); //add or subtract pi by 2 depending on which side of ref line double piSide; //sign of distance determines which side of line we are on if (distanceFromRefLine > 0) { piSide = glm.PIBy2; } else { piSide = -glm.PIBy2; } double widthMinusOverlap; //move the ABLine over based on the overlap amount set in vehicle if (mf.tool.toolOffset != 0) { widthMinusOverlap = mf.tool.toolWidth / 2 - mf.tool.toolOverlap; } else { widthMinusOverlap = mf.tool.toolWidth - mf.tool.toolOverlap; } howManyPathsAway = Math.Round(minDistance / widthMinusOverlap, 0, MidpointRounding.AwayFromZero); curveNumber = howManyPathsAway; if (distanceFromRefLine < 0) { curveNumber = -curveNumber; } //double toolOffset = mf.tool.toolOffset; //build the current line curList?.Clear(); for (int i = 0; i < ptCount; i++) { var point = new vec3( refList[i].easting + (Math.Sin(piSide + aveLineHeading) * ((widthMinusOverlap * howManyPathsAway))), refList[i].northing + (Math.Cos(piSide + aveLineHeading) * ((widthMinusOverlap * howManyPathsAway))), refList[i].heading); curList.Add(point); } double minDistA = 1000000, minDistB = 1000000; ptCount = curList.Count; if (ptCount > 0) { if (mf.isStanleyUsed) { //find the closest 2 points to current fix for (int t = 0; t < ptCount; t++) { dist = ((steer.easting - curList[t].easting) * (steer.easting - curList[t].easting)) + ((steer.northing - curList[t].northing) * (steer.northing - curList[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 = curList[B].easting - curList[A].easting; dz = curList[B].northing - curList[A].northing; if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon) { return; } //abHeading = Math.Atan2(dz, dx); double abHeading = curList[A].heading; //how far from current AB Line is fix distanceFromCurrentLine = ((dz * steer.easting) - (dx * steer.northing) + (curList[B].easting * curList[A].northing) - (curList[B].northing * curList[A].easting)) / Math.Sqrt((dz * dz) + (dx * dx)); //are we on the right side or not isOnRightSideCurrentLine = distanceFromCurrentLine > 0; //absolute the distance distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine); //Subtract the two headings, if > 1.57 its going the opposite heading as refAB double abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading)); if (abFixHeadingDelta >= Math.PI) { abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI); } isABSameAsVehicleHeading = abFixHeadingDelta < glm.PIBy2; // calc point on ABLine closest to current position double U = (((steer.easting - curList[A].easting) * dx) + ((steer.northing - curList[A].northing) * dz)) / ((dx * dx) + (dz * dz)); rEastCu = curList[A].easting + (U * dx); rNorthCu = curList[A].northing + (U * dz); //distance is negative if on left, positive if on right if (isABSameAsVehicleHeading) { if (!isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } abFixHeadingDelta = (steer.heading - abHeading); } //opposite way so right is left else { if (isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } abFixHeadingDelta = (steer.heading - abHeading + Math.PI); } //Fix the circular error if (abFixHeadingDelta > Math.PI) { abFixHeadingDelta -= Math.PI; } else if (abFixHeadingDelta < Math.PI) { abFixHeadingDelta += Math.PI; } if (abFixHeadingDelta > glm.PIBy2) { abFixHeadingDelta -= Math.PI; } else if (abFixHeadingDelta < -glm.PIBy2) { abFixHeadingDelta += Math.PI; } abFixHeadingDelta *= mf.vehicle.stanleyHeadingErrorGain; if (abFixHeadingDelta > 0.74) { abFixHeadingDelta = 0.74; } if (abFixHeadingDelta < -0.74) { abFixHeadingDelta = -0.74; } steerAngleCu = Math.Atan((distanceFromCurrentLine * mf.vehicle.stanleyGain) / ((mf.pn.speed * 0.277777) + 1)); if (steerAngleCu > 0.74) { steerAngleCu = 0.74; } if (steerAngleCu < -0.74) { steerAngleCu = -0.74; } steerAngleCu = glm.toDegrees((steerAngleCu + abFixHeadingDelta) * -1.0); if (steerAngleCu < -mf.vehicle.maxSteerAngle) { steerAngleCu = -mf.vehicle.maxSteerAngle; } if (steerAngleCu > mf.vehicle.maxSteerAngle) { steerAngleCu = mf.vehicle.maxSteerAngle; } //Convert to millimeters distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero); } else { //find the closest 2 points to current fix for (int t = 0; t < ptCount; t++) { dist = ((pivot.easting - curList[t].easting) * (pivot.easting - curList[t].easting)) + ((pivot.northing - curList[t].northing) * (pivot.northing - curList[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 = curList[B].easting - curList[A].easting; dz = curList[B].northing - curList[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 distanceFromCurrentLine = ((dz * pivot.easting) - (dx * pivot.northing) + (curList[B].easting * curList[A].northing) - (curList[B].northing * curList[A].easting)) / Math.Sqrt((dz * dz) + (dx * dx)); //are we on the right side or not isOnRightSideCurrentLine = distanceFromCurrentLine > 0; //absolute the distance distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine); // ** Pure pursuit ** - calc point on ABLine closest to current position double U = (((pivot.easting - curList[A].easting) * dx) + ((pivot.northing - curList[A].northing) * dz)) / ((dx * dx) + (dz * dz)); rEastCu = curList[A].easting + (U * dx); rNorthCu = curList[A].northing + (U * dz); //double minx, maxx, miny, maxy; //minx = Math.Min(curList[A].northing, curList[B].northing); //maxx = Math.Max(curList[A].northing, curList[B].northing); //miny = Math.Min(curList[A].easting, curList[B].easting); //maxy = Math.Max(curList[A].easting, curList[B].easting); //isValid = rNorthCu >= minx && rNorthCu <= maxx && (rEastCu >= miny && rEastCu <= maxy); //if (!isValid) //{ // //invalid distance so tell AS module // distanceFromCurrentLine = 32000; // mf.guidanceLineDistanceOff = 32000; // return; //} //used for accumulating distance to find goal point double distSoFar; //update base on autosteer settings and distance from line double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine); mf.lookaheadActual = goalPointDistance; // used for calculating the length squared of next segment. double tempDist = 0.0; if (!isSameWay) { //counting down isABSameAsVehicleHeading = false; distSoFar = glm.Distance(curList[A], rEastCu, rNorthCu); //Is this segment long enough to contain the full lookahead distance? if (distSoFar > goalPointDistance) { //treat current segment like an AB Line goalPointCu.easting = rEastCu - (Math.Sin(curList[A].heading) * goalPointDistance); goalPointCu.northing = rNorthCu - (Math.Cos(curList[A].heading) * goalPointDistance); } //multiple segments required else { //cycle thru segments and keep adding lengths. check if start and break if so. while (A > 0) { B--; A--; tempDist = glm.Distance(curList[B], curList[A]); //will we go too far? if ((tempDist + distSoFar) > goalPointDistance) { break; //tempDist contains the full length of next segment } else { distSoFar += tempDist; } } double t = (goalPointDistance - distSoFar); // the remainder to yet travel t /= tempDist; goalPointCu.easting = (((1 - t) * curList[B].easting) + (t * curList[A].easting)); goalPointCu.northing = (((1 - t) * curList[B].northing) + (t * curList[A].northing)); } } else { //counting up isABSameAsVehicleHeading = true; distSoFar = glm.Distance(curList[B], rEastCu, rNorthCu); //Is this segment long enough to contain the full lookahead distance? if (distSoFar > goalPointDistance) { //treat current segment like an AB Line goalPointCu.easting = rEastCu + (Math.Sin(curList[A].heading) * goalPointDistance); goalPointCu.northing = rNorthCu + (Math.Cos(curList[A].heading) * goalPointDistance); } //multiple segments required else { //cycle thru segments and keep adding lengths. check if end and break if so. // ReSharper disable once LoopVariableIsNeverChangedInsideLoop while (B < ptCount - 1) { B++; A++; tempDist = glm.Distance(curList[B], curList[A]); //will we go too far? if ((tempDist + distSoFar) > goalPointDistance) { //A--; B--; break; //tempDist contains the full length of next segment } distSoFar += tempDist; } //xt = (((1 - t) * x0 + t * x1) //yt = ((1 - t) * y0 + t * y1)) double t = (goalPointDistance - distSoFar); // the remainder to yet travel t /= tempDist; goalPointCu.easting = (((1 - t) * curList[A].easting) + (t * curList[B].easting)); goalPointCu.northing = (((1 - t) * curList[A].northing) + (t * curList[B].northing)); } } //calc "D" the distance from pivot axle to lookahead point double goalPointDistanceSquared = glm.DistanceSquared(goalPointCu.northing, goalPointCu.easting, pivot.northing, pivot.easting); //calculate the the delta x in local coordinates and steering angle degrees based on wheelbase double localHeading = glm.twoPI - mf.fixHeading; ppRadiusCu = goalPointDistanceSquared / (2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading)))); steerAngleCu = glm.toDegrees(Math.Atan(2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared)); if (steerAngleCu < -mf.vehicle.maxSteerAngle) { steerAngleCu = -mf.vehicle.maxSteerAngle; } if (steerAngleCu > mf.vehicle.maxSteerAngle) { steerAngleCu = mf.vehicle.maxSteerAngle; } if (ppRadiusCu < -500) { ppRadiusCu = -500; } if (ppRadiusCu > 500) { ppRadiusCu = 500; } radiusPointCu.easting = pivot.easting + (ppRadiusCu * Math.Cos(localHeading)); radiusPointCu.northing = pivot.northing + (ppRadiusCu * 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(steerAngleCu))) / mf.vehicle.wheelbase; //clamp the steering angle to not exceed safe angular velocity if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity) { steerAngleCu = glm.toDegrees(steerAngleCu > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))) : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))); } //Convert to centimeters distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero); //distance is negative if on left, positive if on right //if you're going the opposite direction left is right and right is left //double temp; if (isABSameAsVehicleHeading) { //temp = (abHeading); if (!isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } } //opposite way so right is left else { //temp = (abHeading - Math.PI); //if (temp < 0) temp = (temp + glm.twoPI); //temp = glm.toDegrees(temp); if (isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } } } mf.guidanceLineDistanceOff = mf.distanceDisplay = (Int16)distanceFromCurrentLine; mf.guidanceLineSteerAngle = (Int16)(steerAngleCu * 100); if (mf.yt.isYouTurnTriggered) { //do the pure pursuit from youTurn mf.yt.DistanceFromYouTurnLine(); mf.seq.DoSequenceEvent(); //now substitute what it thinks are AB line values with auto turn values steerAngleCu = mf.yt.steerAngleYT; distanceFromCurrentLine = mf.yt.distanceFromCurrentLine; goalPointCu = mf.yt.goalPointYT; radiusPointCu.easting = mf.yt.radiusPointYT.easting; radiusPointCu.northing = mf.yt.radiusPointYT.northing; ppRadiusCu = mf.yt.ppRadiusYT; } } else { //invalid distance so tell AS module distanceFromCurrentLine = 32000; mf.guidanceLineDistanceOff = 32000; } }
public void GetCurrentABLine(vec3 pivot) { //move the ABLine over based on the overlap amount set in vehicle double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap; //x2-x1 double dx = refABLineP2.easting - refABLineP1.easting; //z2-z1 double dy = refABLineP2.northing - refABLineP1.northing; //how far are we away from the reference line at 90 degrees distanceFromRefLine = ((dy * pivot.easting) - (dx * pivot.northing) + (refABLineP2.easting * refABLineP1.northing) - (refABLineP2.northing * refABLineP1.easting)) / Math.Sqrt((dy * dy) + (dx * dx)); //sign of distance determines which side of line we are on if (distanceFromRefLine > 0) { refLineSide = 1; } else { refLineSide = -1; } //absolute the distance distanceFromRefLine = Math.Abs(distanceFromRefLine); //Which ABLine is the vehicle on, negative is left and positive is right side howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero); //generate that pass number as signed integer passNumber = Convert.ToInt32(refLineSide * howManyPathsAway); //calculate the new point that is number of implement widths over double toolOffset = mf.vehicle.toolOffset; vec2 point1; //depending which way you are going, the offset can be either side if (isABSameAsVehicleHeading) { point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting, (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing); } else { point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting, (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing); } //create the new line extent points for current ABLine based on original heading of AB line currentABLineP1.easting = point1.easting - (Math.Sin(abHeading) * 40000.0); currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0); currentABLineP2.easting = point1.easting + (Math.Sin(abHeading) * 40000.0); currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0); //get the distance from currently active AB line //x2-x1 dx = currentABLineP2.easting - currentABLineP1.easting; //z2-z1 dy = currentABLineP2.northing - currentABLineP1.northing; //save a copy of dx,dy in youTurn mf.yt.dxAB = dx; mf.yt.dyAB = dy; //how far from current AB Line is fix distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting)) / Math.Sqrt((dy * dy) + (dx * dx)); //are we on the right side or not isOnRightSideCurrentLine = distanceFromCurrentLine > 0; // filter for distance if (Properties.Settings.Default.is_xte) { distancefilter = ((Properties.Settings.Default.xtefilter * distancefilter) + distanceFromCurrentLine) / (Properties.Settings.Default.xtefilter + 1); distanceFromCurrentLine = distancefilter; } //absolute the distance distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine); //double goalPointDistance = mf.vehicle.goalPointLookAhead; //if (distanceFromCurrentLine < mf.vehicle.goalPointDistanceFromLine ) // goalPointDistance -= goalPointDistance * (mf.vehicle.goalPointDistanceFromLine - distanceFromCurrentLine); //if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum) goalPointDistance = mf.vehicle.goalPointLookAheadMinimum; if (Properties.Settings.Default.isortner) { goalPointDistance = (mf.pn.speed - distanceFromCurrentLine * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead; // goalPointLookAhead should be 10-20 if (distanceFromCurrentLine > 0.4) { goalPointDistance = (mf.pn.speed - 0.4 * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead;; goalPointDistance += ((distanceFromCurrentLine - 0.4) * Properties.Settings.Default.minuslookahedortner) * speedmaxlahead; if (goalPointDistance > mf.pn.speed * speedmaxlahead) { goalPointDistance = mf.pn.speed * speedmaxlahead; } } if (goalPointDistance < mf.pn.speed * speedminlahead) { goalPointDistance = mf.pn.speed * speedminlahead; } } else if (Properties.Settings.Default.isspeed) { goalPointDistance = mf.pn.speed * speedmaxlahead; if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum) { goalPointDistance = mf.vehicle.goalPointLookAheadMinimum; } } else if (Properties.Settings.Default.isschelter) { //!!!!!how far should goal point be away //!!!!!double goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777); //!!!!!if (goalPointDistance < mf.vehicle.minLookAheadDistance) goalPointDistance = mf.vehicle.minLookAheadDistance; //!!!!!Versuch: how far should goal point be away if (distanceFromCurrentLine < 0.3) { goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) - (distanceFromCurrentLine * 12.5 * (mf.pn.speed / 10)); } if (distanceFromCurrentLine >= 0.3 & distanceFromCurrentLine < 0.6) { goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) - (((0.3 - (distanceFromCurrentLine - 0.3)) * 12.5 * (mf.pn.speed / 10))); } if (distanceFromCurrentLine >= 0.6 & distanceFromCurrentLine < 1) { goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777); } if (distanceFromCurrentLine > 1 & distanceFromCurrentLine < 2.5) { goalPointDistance = ((mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) * (((distanceFromCurrentLine - 1) / 10) + 1)); } if (distanceFromCurrentLine >= 2.5) { goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777) * 1.15; } //minimum of 3.0 meters look ahead if (goalPointDistance < speedminlahead) { goalPointDistance = speedminlahead; } } else { //how far should goal point be away - speed * seconds * kmph -> m/s then limit min value goalPointDistance = mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.27777777; if (distanceFromCurrentLine < 1.0) { goalPointDistance += distanceFromCurrentLine * goalPointDistance * mf.vehicle.goalPointDistanceMultiplier; } else { goalPointDistance += goalPointDistance * mf.vehicle.goalPointDistanceMultiplier; } if (goalPointDistance < mf.vehicle.goalPointLookAheadMinimum) { goalPointDistance = mf.vehicle.goalPointLookAheadMinimum; } } mf.test1 = goalPointDistance; //Subtract the two headings, if > 1.57 its going the opposite heading as refAB abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading)); if (abFixHeadingDelta >= Math.PI) { abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI); } // ** Pure pursuit ** - calc point on ABLine closest to current position double U = (((pivot.easting - currentABLineP1.easting) * dx) + ((pivot.northing - currentABLineP1.northing) * dy)) / ((dx * dx) + (dy * dy)); //point on AB line closest to pivot axle point rEastAB = currentABLineP1.easting + (U * dx); rNorthAB = currentABLineP1.northing + (U * dy); //how far should goal point be away - speed * seconds * kmph -> m/s + min value //double goalPointDistance = (mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.2777777777); //if (goalPointDistance < mf.vehicle.minLookAheadDistance) goalPointDistance = mf.vehicle.minLookAheadDistance; if (abFixHeadingDelta >= glm.PIBy2) { isABSameAsVehicleHeading = false; goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance); goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance); } else { isABSameAsVehicleHeading = true; goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance); goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance); } //calc "D" the distance from pivot axle to lookahead point double goalPointDistanceDSquared = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting); //calculate the the new x in local coordinates and steering angle degrees based on wheelbase double localHeading = glm.twoPI - mf.fixHeading; ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading)))); //make sure pp doesn't generate a radius smaller then turn radius //if (ppRadiusAB > 0) //{ // if (ppRadiusAB < mf.vehicle.minTurningRadius * 0.95) ppRadiusAB = mf.vehicle.minTurningRadius * 0.95; //} //else if (ppRadiusAB > -mf.vehicle.minTurningRadius * 0.95) //{ // ppRadiusAB = -mf.vehicle.minTurningRadius * 0.95; //} steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceDSquared)); if (steerAngleAB < -mf.vehicle.maxSteerAngle) { steerAngleAB = -mf.vehicle.maxSteerAngle; } if (steerAngleAB > mf.vehicle.maxSteerAngle) { steerAngleAB = mf.vehicle.maxSteerAngle; } //limit circle size for display purpose if (ppRadiusAB < -500) { ppRadiusAB = -500; } if (ppRadiusAB > 500) { ppRadiusAB = 500; } radiusPointAB.easting = pivot.easting + (ppRadiusAB * Math.Cos(localHeading)); radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading)); //Convert to millimeters distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero); //angular velocity in rads/sec = 2PI * m/sec * radians/meters angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase; //clamp the steering angle to not exceed safe angular velocity if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity) { steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))) : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))); } //distance is negative if on left, positive if on right if (isABSameAsVehicleHeading) { if (!isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } } //opposite way so right is left else { if (isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } } mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine; mf.guidanceLineSteerAngle = (Int16)(steerAngleAB * 100); if (mf.yt.isYouTurnShapeDisplayed) { //do the pure pursuit from youTurn mf.yt.DistanceFromYouTurnLine(); //now substitute what it thinks are AB line values with auto turn values steerAngleAB = mf.yt.steerAngleYT; distanceFromCurrentLine = mf.yt.distanceFromCurrentLine; goalPointAB = mf.yt.goalPointYT; radiusPointAB.easting = mf.yt.radiusPointYT.easting; radiusPointAB.northing = mf.yt.radiusPointYT.northing; ppRadiusAB = mf.yt.ppRadiusYT; } }
//every time a new fix, a new patch point from last point to this point //only need prev point on the first points of triangle strip that makes a box (2 triangles) public void AddMappingPoint() { //add two triangles for next step. //left side vec3 point = new vec3(leftPoint.easting, leftPoint.northing, 0); //add the point to List triangleList.Add(point); //Right side vec3 point2 = new vec3(rightPoint.easting, rightPoint.northing, 0); //add the point to the list triangleList.Add(point2); //count the triangle pairs numTriangles++; //quick count int c = triangleList.Count - 1; //when closing a job the triangle patches all are emptied but the section delay keeps going. //Prevented by quick check. 4 points plus colour if (c >= 5) { //calculate area of these 2 new triangles - AbsoluteValue of (Ax(By-Cy) + Bx(Cy-Ay) + Cx(Ay-By)/2) { double temp = (triangleList[c].easting * (triangleList[c - 1].northing - triangleList[c - 2].northing)) + (triangleList[c - 1].easting * (triangleList[c - 2].northing - triangleList[c].northing)) + (triangleList[c - 2].easting * (triangleList[c].northing - triangleList[c - 1].northing)); temp = Math.Abs(temp / 2.0); mf.fd.workedAreaTotal += temp; mf.fd.workedAreaTotalUser += temp; //temp = 0; temp = (triangleList[c - 1].easting * (triangleList[c - 2].northing - triangleList[c - 3].northing)) + (triangleList[c - 2].easting * (triangleList[c - 3].northing - triangleList[c - 1].northing)) + (triangleList[c - 3].easting * (triangleList[c - 1].northing - triangleList[c - 2].northing)); temp = Math.Abs(temp / 2.0); mf.fd.workedAreaTotal += temp; mf.fd.workedAreaTotalUser += temp; } } if (numTriangles > 36) { numTriangles = 0; //save the cutoff patch to be saved later mf.patchSaveList.Add(triangleList); triangleList = new List <vec3>(); patchList.Add(triangleList); //Add Patch colour vec3 colur = new vec3(mf.sectionColorDay.R, mf.sectionColorDay.G, mf.sectionColorDay.B); triangleList.Add(colur); //add the points to List, yes its more points, but breaks up patches for culling triangleList.Add(point); triangleList.Add(point2); } }
//determine closest point on left side public void BuildContourGuidanceLine(vec3 pivot) { double toolWid = mf.vehicle.toolWidth; double sinH = Math.Sin(pivot.heading) * 2.0 * toolWid; double cosH = Math.Cos(pivot.heading) * 2.0 * toolWid; double sin2HL = 0; double cos2HL = 0; double sin2HR = 0; double cos2HR = 0; if (mf.vehicle.toolOffset < 0) { //sticks out more left sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2))); cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2))); sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset))); cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset))); } else { //sticks out more right sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset))); cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset))); sin2HR = Math.Sin(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2))); cos2HR = Math.Cos(pivot.heading + glm.PIBy2) * (1.33 * (toolWid + Math.Abs(mf.vehicle.toolOffset * 2))); } //narrow equipment needs bigger bounding box. if (mf.vehicle.toolWidth < 6) { sinH = Math.Sin(pivot.heading) * 4 * toolWid; cosH = Math.Cos(pivot.heading) * 4 * toolWid; } double sin3H = Math.Sin(pivot.heading + glm.PIBy2) * 0.5; double cos3H = Math.Cos(pivot.heading + glm.PIBy2) * 0.5; //build a frustum box ahead of fix to find adjacent paths and points //left boxA.easting = pivot.easting - sin2HL; boxA.northing = pivot.northing - cos2HL; boxA.easting -= (sinH * 0.25); //bottom left outside boxA.northing -= (cosH * 0.25); boxD.easting = boxA.easting + sinH; //top left outside boxD.northing = boxA.northing + cosH; boxE.easting = pivot.easting - sin3H; // inside bottom boxE.northing = pivot.northing - cos3H; boxG.easting = boxE.easting + (sinH * 0.3); //inside top boxG.northing = boxE.northing + (cosH * 0.3); //right boxB.easting = pivot.easting + sin2HR; boxB.northing = pivot.northing + cos2HR; boxB.easting -= (sinH * 0.25); boxB.northing -= (cosH * 0.25); boxC.easting = boxB.easting + sinH; boxC.northing = boxB.northing + cosH; boxF.easting = pivot.easting + sin3H; boxF.northing = pivot.northing + cos3H; boxH.easting = boxF.easting + (sinH * 0.3); //inside top boxH.northing = boxF.northing + (cosH * 0.3); conList.Clear(); ctList.Clear(); int ptCount; //check if no strips yet, return int stripCount = stripList.Count; if (stripCount == 0) return; cvec pointC = new cvec(); if (isRightPriority) { //determine if points are in right side frustum box for (int s = 0; s < stripCount; s++) { ptCount = stripList[s].Count; for (int p = 0; p < ptCount; p++) { //FHCBF if ((((boxH.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing)) - ((boxH.northing - boxC.northing) * (stripList[s][p].easting - boxC.easting))) < 0) { continue; } if ((((boxC.easting - boxB.easting) * (stripList[s][p].northing - boxB.northing)) - ((boxC.northing - boxB.northing) * (stripList[s][p].easting - boxB.easting))) < 0) { continue; } if ((((boxB.easting - boxF.easting) * (stripList[s][p].northing - boxF.northing)) - ((boxB.northing - boxF.northing) * (stripList[s][p].easting - boxF.easting))) < 0) { continue; } if ((((boxF.easting - boxH.easting) * (stripList[s][p].northing - boxH.northing)) - ((boxF.northing - boxH.northing) * (stripList[s][p].easting - boxH.easting))) < 0) { continue; } //in the box so is it parallelish or perpedicularish to current heading ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI); if (ref2 < 1.2 || ref2 > 1.9) { //it's in the box and parallelish so add to list pointC.x = stripList[s][p].easting; pointC.z = stripList[s][p].northing; pointC.h = stripList[s][p].heading; pointC.strip = s; pointC.pt = p; conList.Add(pointC); } } } if (conList.Count == 0) { //determine if points are in frustum box for (int s = 0; s < stripCount; s++) { ptCount = stripList[s].Count; for (int p = 0; p < ptCount; p++) { //EADGE if ((((boxG.easting - boxE.easting) * (stripList[s][p].northing - boxE.northing)) - ((boxG.northing - boxE.northing) * (stripList[s][p].easting - boxE.easting))) < 0) { continue; } if ((((boxE.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing)) - ((boxE.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; } if ((((boxA.easting - boxD.easting) * (stripList[s][p].northing - boxD.northing)) - ((boxA.northing - boxD.northing) * (stripList[s][p].easting - boxD.easting))) < 0) { continue; } if ((((boxD.easting - boxG.easting) * (stripList[s][p].northing - boxG.northing)) - ((boxD.northing - boxG.northing) * (stripList[s][p].easting - boxG.easting))) < 0) { continue; } //in the box so is it parallelish or perpedicularish to current heading ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI); if (ref2 < 1.2 || ref2 > 1.9) { //it's in the box and parallelish so add to list pointC.x = stripList[s][p].easting; pointC.z = stripList[s][p].northing; pointC.h = stripList[s][p].heading; pointC.strip = s; pointC.pt = p; conList.Add(pointC); } } } } } else { for (int s = 0; s < stripCount; s++) { ptCount = stripList[s].Count; for (int p = 0; p < ptCount; p++) { //EADGE if ((((boxG.easting - boxE.easting) * (stripList[s][p].northing - boxE.northing)) - ((boxG.northing - boxE.northing) * (stripList[s][p].easting - boxE.easting))) < 0) { continue; } if ((((boxE.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing)) - ((boxE.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; } if ((((boxA.easting - boxD.easting) * (stripList[s][p].northing - boxD.northing)) - ((boxA.northing - boxD.northing) * (stripList[s][p].easting - boxD.easting))) < 0) { continue; } if ((((boxD.easting - boxG.easting) * (stripList[s][p].northing - boxG.northing)) - ((boxD.northing - boxG.northing) * (stripList[s][p].easting - boxG.easting))) < 0) { continue; } //in the box so is it parallelish or perpedicularish to current heading ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI); if (ref2 < 1.2 || ref2 > 1.9) { //it's in the box and parallelish so add to list pointC.x = stripList[s][p].easting; pointC.z = stripList[s][p].northing; pointC.h = stripList[s][p].heading; pointC.strip = s; pointC.pt = p; conList.Add(pointC); } } } if (conList.Count == 0) { //determine if points are in frustum box for (int s = 0; s < stripCount; s++) { ptCount = stripList[s].Count; for (int p = 0; p < ptCount; p++) { if ((((boxH.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing)) - ((boxH.northing - boxC.northing) * (stripList[s][p].easting - boxC.easting))) < 0) { continue; } if ((((boxC.easting - boxB.easting) * (stripList[s][p].northing - boxB.northing)) - ((boxC.northing - boxB.northing) * (stripList[s][p].easting - boxB.easting))) < 0) { continue; } if ((((boxB.easting - boxF.easting) * (stripList[s][p].northing - boxF.northing)) - ((boxB.northing - boxF.northing) * (stripList[s][p].easting - boxF.easting))) < 0) { continue; } if ((((boxF.easting - boxH.easting) * (stripList[s][p].northing - boxH.northing)) - ((boxF.northing - boxH.northing) * (stripList[s][p].easting - boxH.easting))) < 0) { continue; } //in the box so is it parallelish or perpedicularish to current heading ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI); if (ref2 < 1.2 || ref2 > 1.9) { //it's in the box and parallelish so add to list pointC.x = stripList[s][p].easting; pointC.z = stripList[s][p].northing; pointC.h = stripList[s][p].heading; pointC.strip = s; pointC.pt = p; conList.Add(pointC); } } } } } //no points in the box, exit ptCount = conList.Count; if (ptCount == 0) { distanceFromCurrentLine = 9999; distanceFromCurrentLine = 32000; mf.guidanceLineDistanceOff = 32000; return; } //determine closest point minDistance = 99999; for (int i = 0; i < ptCount; i++) { double dist = ((pivot.easting - conList[i].x) * (pivot.easting - conList[i].x)) + ((pivot.northing - conList[i].z) * (pivot.northing - conList[i].z)); if (minDistance >= dist) { minDistance = dist; closestRefPoint = i; } } //now we have closest point, the distance squared from it, and which patch and point its from int strip = conList[closestRefPoint].strip; int pt = conList[closestRefPoint].pt; refX = stripList[strip][pt].easting; refZ = stripList[strip][pt].northing; refHeading = stripList[strip][pt].heading; //are we going same direction as stripList was created? bool isSameWay = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - refHeading) - Math.PI) < 1.4; //which side of the patch are we on is next //calculate endpoints of reference line based on closest point refPoint1.easting = refX - (Math.Sin(refHeading) * 50.0); refPoint1.northing = refZ - (Math.Cos(refHeading) * 50.0); refPoint2.easting = refX + (Math.Sin(refHeading) * 50.0); refPoint2.northing = refZ + (Math.Cos(refHeading) * 50.0); //x2-x1 double dx = refPoint2.easting - refPoint1.easting; //z2-z1 double dz = refPoint2.northing - refPoint1.northing; //how far are we away from the reference line at 90 degrees - 2D cross product and distance distanceFromRefLine = ((dz * mf.pn.fix.easting) - (dx * mf.pn.fix.northing) + (refPoint2.easting * refPoint1.northing) - (refPoint2.northing * refPoint1.easting)) / Math.Sqrt((dz * dz) + (dx * dx)); //add or subtract pi by 2 depending on which side of ref line double piSide; //sign of distance determines which side of line we are on if (distanceFromRefLine > 0) piSide = glm.PIBy2; else piSide = -glm.PIBy2; //offset calcs double toolOffset = mf.vehicle.toolOffset; if (isSameWay) { toolOffset = 0.0; } else { if (distanceFromRefLine > 0) toolOffset *= 2.0; else toolOffset *= -2.0; } //move the Guidance Line over based on the overlap, width, and offset amount set in vehicle double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap + toolOffset; //absolute the distance distanceFromRefLine = Math.Abs(distanceFromRefLine); //make the new guidance line list called guideList ptCount = stripList[strip].Count - 1; int start, stop; start = pt - 35; if (start < 0) start = 0; stop = pt + 35; if (stop > ptCount) stop = ptCount + 1; double distSq = widthMinusOverlap * widthMinusOverlap * 0.875; bool fail = false; for (int i = start; i < stop; i++) { //var point = new vec3( // stripList[strip][i].easting + (Math.Sin(piSide + stripList[strip][i].heading) * widthMinusOverlap), // stripList[strip][i].northing + (Math.Cos(piSide + stripList[strip][i].heading) * widthMinusOverlap), // stripList[strip][i].heading); //ctList.Add(point); var point = new vec3( stripList[strip][i].easting + (Math.Sin(piSide + stripList[strip][i].heading) * widthMinusOverlap), stripList[strip][i].northing + (Math.Cos(piSide + stripList[strip][i].heading) * widthMinusOverlap), stripList[strip][i].heading); //ctList.Add(point); //make sure its not closer then 1 eq width for (int j = start; j < stop; j++) { double check = glm.DistanceSquared(point.northing, point.easting, stripList[strip][j].northing, stripList[strip][j].easting); if (check < distSq) { fail = true; break; } } if (!fail) ctList.Add(point); fail = false; } //if no boundaries, just return. if (!mf.bnd.bndArr[0].isSet) return; int ctCount = ctList.Count; if (ctCount < 6) return; const double spacing = 0.8; double distance; for (int i = 0; i < ctCount - 1; i++) { distance = glm.Distance(ctList[i], ctList[i + 1]); if (distance < spacing) { ctList.RemoveAt(i + 1); ctCount = ctList.Count; i--; } } { //count the reference list of original curve int cnt = ctList.Count; //just go back if not very long if (cnt < 10) return; //the temp array vec3[] arr = new vec3[cnt]; //how many samples const int smPts = 2; //read the points before and after the setpoint for (int s = 0; s < smPts; s++) { arr[s].easting = ctList[s].easting; arr[s].northing = ctList[s].northing; arr[s].heading = ctList[s].heading; } for (int s = cnt - smPts; s < cnt; s++) { arr[s].easting = ctList[s].easting; arr[s].northing = ctList[s].northing; arr[s].heading = ctList[s].heading; } //average them - center weighted average for (int i = smPts; i < cnt - smPts; i++) { for (int j = -smPts; j < smPts; j++) { arr[i].easting += ctList[j + i].easting; arr[i].northing += ctList[j + i].northing; } arr[i].easting /= (smPts * 2); arr[i].northing /= (smPts * 2); arr[i].heading = ctList[i].heading; } //make a list to draw ctList?.Clear(); for (int i = 0; i < cnt; i++) { ctList.Add(arr[i]); } } }
public void FixBoundaryLine(int bndNum, double spacing) { //count the points from the boundary int ptCount = bndLine.Count; //boundary point spacing based on eq width spacing *= 0.25; if (spacing < 1) { spacing = 1; } if (spacing > 3) { spacing = 3; } //first find out which side is inside the boundary double oneSide = glm.PIBy2; vec3 point = new vec3(bndLine[3].easting - (Math.Sin(oneSide + bndLine[3].heading) * 2.0), bndLine[3].northing - (Math.Cos(oneSide + bndLine[3].heading) * 2.0), 0.0); //make sure boundaries are wound correctly if (bndNum == 0) { //outside an outer boundary means its wound clockwise if (!IsPointInsideBoundary(point)) { ReverseWinding(); } } else { //inside an inner boundary means its wound clockwise if (IsPointInsideBoundary(point)) { ReverseWinding(); } spacing *= 0.66; } //make sure distance isn't too small between points on headland int bndCount = bndLine.Count; double distance; for (int i = 0; i < bndCount - 1; i++) { distance = glm.Distance(bndLine[i], bndLine[i + 1]); if (distance < spacing) { bndLine.RemoveAt(i + 1); bndCount = bndLine.Count; i--; } } //make sure distance isn't too big between points on boundary bndCount = bndLine.Count; spacing *= 1.33; for (int i = 0; i < bndCount; i++) { int j = i + 1; if (j == bndCount) { j = 0; } distance = glm.Distance(bndLine[i], bndLine[j]); if (distance > spacing) { CBndPt pointB = new CBndPt((bndLine[i].easting + bndLine[j].easting) / 2.0, (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading); bndLine.Insert(j, pointB); bndCount = bndLine.Count; i--; } } //make sure distance isn't too big between points on boundary bndCount = bndLine.Count; spacing *= 1.33; for (int i = 0; i < bndCount; i++) { int j = i + 1; if (j == bndCount) { j = 0; } distance = glm.Distance(bndLine[i], bndLine[j]); if (distance > spacing) { CBndPt pointB = new CBndPt((bndLine[i].easting + bndLine[j].easting) / 2.0, (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading); bndLine.Insert(j, pointB); bndCount = bndLine.Count; i--; } } //make sure headings are correct for calculated points CalculateBoundaryHeadings(); }
public void FindClosestTurnPoint(bool isYouTurnRight, vec3 fromPt, double headAB) { //initial scan is straight ahead of pivot point of vehicle to find the right turnLine/boundary vec3 pt = new vec3(); vec3 rayPt = new vec3(); int closestTurnNum = 99; double cosHead = Math.Cos(headAB); double sinHead = Math.Sin(headAB); for (int b = 1; b < mf.maxCrossFieldLength; b += 2) { pt.easting = fromPt.easting + (sinHead * b); pt.northing = fromPt.northing + (cosHead * b); int idx = IsPointInsideTurnArea(pt); if (idx != 0) { closestTurnNum = idx; rayPt.easting = pt.easting; rayPt.northing = pt.northing; break; } } if (closestTurnNum < 0) { closestTurnNum = 0; } //second scan is straight ahead of outside of tool based on turn direction double scanWidthL, scanWidthR; if (isYouTurnRight) //its actually left { scanWidthL = -(mf.tool.toolWidth * 0.25) - (mf.tool.toolWidth * 0.5); scanWidthR = (mf.tool.toolWidth * 0.25) - (mf.tool.toolWidth * 0.5); } else { scanWidthL = -(mf.tool.toolWidth * 0.25) + (mf.tool.toolWidth * 0.5); scanWidthR = (mf.tool.toolWidth * 0.25) + (mf.tool.toolWidth * 0.5); } //isYouTurnRight actuall means turning left - Painful, but it switches later boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthL); boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthL); boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthR); boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthR); boxC.easting = boxB.easting + (Math.Sin(headAB) * boxLength); boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength); boxD.easting = boxA.easting + (Math.Sin(headAB) * boxLength); boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength); //determine if point is inside bounding box of the 1 turn chosen above turnClosestList.Clear(); vec4 inBox; int ptCount = bndList[closestTurnNum].turnLine.Count; for (int p = 0; p < ptCount; p++) { if ((((boxB.easting - boxA.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxA.northing)) - ((boxB.northing - boxA.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxA.easting))) < 0) { continue; } if ((((boxD.easting - boxC.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxC.northing)) - ((boxD.northing - boxC.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxC.easting))) < 0) { continue; } if ((((boxC.easting - boxB.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxB.northing)) - ((boxC.northing - boxB.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxB.easting))) < 0) { continue; } if ((((boxA.easting - boxD.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxD.northing)) - ((boxA.northing - boxD.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxD.easting))) < 0) { continue; } //it's in the box, so add to list inBox.easting = bndList[closestTurnNum].turnLine[p].easting; inBox.northing = bndList[closestTurnNum].turnLine[p].northing; inBox.heading = bndList[closestTurnNum].turnLine[p].heading; inBox.index = closestTurnNum; //which turn/headland is it from turnClosestList.Add(inBox); } if (turnClosestList.Count == 0) { if (isYouTurnRight) //its actually left { scanWidthL = -(mf.tool.toolWidth * 0.5); scanWidthR = 0; } else { scanWidthL = 0; scanWidthR = (mf.tool.toolWidth * 0.5); } //isYouTurnRight actuall means turning left - Painful, but it switches later boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthL); boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthL); boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidthR); boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidthR); boxC.easting = boxB.easting + (Math.Sin(headAB) * boxLength); boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength); boxD.easting = boxA.easting + (Math.Sin(headAB) * boxLength); boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength); //determine if point is inside bounding box of the 1 turn chosen above turnClosestList.Clear(); ptCount = bndList[closestTurnNum].turnLine.Count; for (int p = 0; p < ptCount; p++) { if ((((boxB.easting - boxA.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxA.northing)) - ((boxB.northing - boxA.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxA.easting))) < 0) { continue; } if ((((boxD.easting - boxC.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxC.northing)) - ((boxD.northing - boxC.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxC.easting))) < 0) { continue; } if ((((boxC.easting - boxB.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxB.northing)) - ((boxC.northing - boxB.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxB.easting))) < 0) { continue; } if ((((boxA.easting - boxD.easting) * (bndList[closestTurnNum].turnLine[p].northing - boxD.northing)) - ((boxA.northing - boxD.northing) * (bndList[closestTurnNum].turnLine[p].easting - boxD.easting))) < 0) { continue; } //it's in the box, so add to list inBox.easting = bndList[closestTurnNum].turnLine[p].easting; inBox.northing = bndList[closestTurnNum].turnLine[p].northing; inBox.heading = bndList[closestTurnNum].turnLine[p].heading; inBox.index = closestTurnNum; //which turn/headland is it from turnClosestList.Add(inBox); } } //which of the points is closest //closestTurnPt.easting = -20000; closestTurnPt.northing = -20000; ptCount = turnClosestList.Count; if (ptCount != 0) { double totalDist = 0.75 * Math.Sqrt(((fromPt.easting - rayPt.easting) * (fromPt.easting - rayPt.easting)) + ((fromPt.northing - rayPt.northing) * (fromPt.northing - rayPt.northing))); //determine closest point double minDistance = 9999999; for (int i = 0; i < ptCount; i++) { double dist = Math.Sqrt(((fromPt.easting - turnClosestList[i].easting) * (fromPt.easting - turnClosestList[i].easting)) + ((fromPt.northing - turnClosestList[i].northing) * (fromPt.northing - turnClosestList[i].northing))); //double distRay = ((rayPt.easting - turnClosestList[i].easting) * (rayPt.easting - turnClosestList[i].easting)) // + ((rayPt.northing - turnClosestList[i].northing) * (rayPt.northing - turnClosestList[i].northing)); if (minDistance >= dist && dist > totalDist) { minDistance = dist; closestTurnPt.easting = turnClosestList[i].easting; closestTurnPt.northing = turnClosestList[i].northing; closestTurnPt.heading = turnClosestList[i].heading; } } if (closestTurnPt.heading < 0) { closestTurnPt.heading += glm.twoPI; } } }
public static double Distance(vec3 first, double east, double north) { return(Math.Sqrt( Math.Pow(first.easting - east, 2) + Math.Pow(first.northing - north, 2))); }
public void FindClosestHeadlandPoint(vec3 fromPt, double headAB, int bndNum) { //heading is based on ABLine, average Curve, and going same direction as AB or not //Draw a bounding box to determine if points are in it if (mf.yt.isYouTurnTriggered || mf.yt.isEnteringDriveThru || mf.yt.isExitingDriveThru) { boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * -scanWidth); //subtract if positive boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * -scanWidth); boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidth); boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidth); boxC.easting = boxB.easting + (Math.Sin(headAB) * boxLength); boxC.northing = boxB.northing + (Math.Cos(headAB) * boxLength); boxD.easting = boxA.easting + (Math.Sin(headAB) * boxLength); boxD.northing = boxA.northing + (Math.Cos(headAB) * boxLength); boxA.easting -= (Math.Sin(headAB) * boxLength); boxA.northing -= (Math.Cos(headAB) * boxLength); boxB.easting -= (Math.Sin(headAB) * boxLength); boxB.northing -= (Math.Cos(headAB) * boxLength); } else { boxA.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * -scanWidth); boxA.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * -scanWidth); boxB.easting = fromPt.easting + (Math.Sin(headAB + glm.PIBy2) * scanWidth); boxB.northing = fromPt.northing + (Math.Cos(headAB + glm.PIBy2) * scanWidth); boxC.easting = boxB.easting + (Math.Sin(headAB) * 2000.0); boxC.northing = boxB.northing + (Math.Cos(headAB) * 2000.0); boxD.easting = boxA.easting + (Math.Sin(headAB) * 2000.0); boxD.northing = boxA.northing + (Math.Cos(headAB) * 2000.0); } //determine if point is inside bounding box int ptCount; hdClosestList.Clear(); vec3 inBox; ptCount = mf.hlArr[bndNum].hlLine.Count; for (int p = 0; p < ptCount; p++) { if ((((boxB.easting - boxA.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxA.northing)) - ((boxB.northing - boxA.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxA.easting))) < 0) { continue; } if ((((boxD.easting - boxC.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxC.northing)) - ((boxD.northing - boxC.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxC.easting))) < 0) { continue; } if ((((boxC.easting - boxB.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxB.northing)) - ((boxC.northing - boxB.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxB.easting))) < 0) { continue; } if ((((boxA.easting - boxD.easting) * (mf.hlArr[bndNum].hlLine[p].northing - boxD.northing)) - ((boxA.northing - boxD.northing) * (mf.hlArr[bndNum].hlLine[p].easting - boxD.easting))) < 0) { continue; } //it's in the box, so add to list inBox = mf.hlArr[bndNum].hlLine[p]; //which boundary/headland is it from hdClosestList.Add(inBox); } //which of the points is closest closestHeadlandPt.easting = -20000; closestHeadlandPt.northing = -20000; ptCount = hdClosestList.Count; if (ptCount != 0) { //determine closest point double minDistance = 9999999; for (int i = 0; i < ptCount; i++) { double dist = ((fromPt.easting - hdClosestList[i].easting) * (fromPt.easting - hdClosestList[i].easting)) + ((fromPt.northing - hdClosestList[i].northing) * (fromPt.northing - hdClosestList[i].northing)); if (minDistance >= dist) { minDistance = dist; closestHeadlandPt.easting = hdClosestList[i].easting; closestHeadlandPt.northing = hdClosestList[i].northing; closestHeadlandPt.heading = hdClosestList[i].heading; } } if (closestHeadlandPt.heading < 0) { closestHeadlandPt.heading += glm.twoPI; } } }
public static double DistanceSquared(vec3 first, vec3 second) { return( Math.Pow(first.easting - second.easting, 2) + Math.Pow(first.northing - second.northing, 2)); }
public void BuildHeadland(double widthSet, int inBndNum) { //count the points from the boundary int ptCount = mf.bndArr[inBndNum].bndLine.Count; //first find out which side is inside the boundary oneSide = glm.PIBy2; vec3 point = new vec3(mf.bndArr[inBndNum].bndLine[3].easting - (Math.Sin(oneSide + mf.bndArr[inBndNum].bndLine[3].heading) * 2.0), mf.bndArr[inBndNum].bndLine[3].northing - (Math.Cos(oneSide + mf.bndArr[inBndNum].bndLine[3].heading) * 2.0), 0.0); if (inBndNum == 0 && !mf.bndArr[inBndNum].IsPointInsideBoundary(point)) { oneSide *= -1.0; } if (inBndNum != 0 && mf.bndArr[inBndNum].IsPointInsideBoundary(point)) { oneSide *= -1.0; } //clear the headland point list hlLine.Clear(); //determine how wide a headland space double totalHeadWidth = (mf.vehicle.toolWidth - mf.vehicle.toolOverlap) * widthSet; for (int i = 0; i < ptCount; i++) { //calculate the point inside the boundary point.easting = mf.bndArr[inBndNum].bndLine[i].easting - (Math.Sin(oneSide + mf.bndArr[inBndNum].bndLine[i].heading) * totalHeadWidth); point.northing = mf.bndArr[inBndNum].bndLine[i].northing - (Math.Cos(oneSide + mf.bndArr[inBndNum].bndLine[i].heading) * totalHeadWidth); point.heading = mf.bndArr[inBndNum].bndLine[i].heading; //only add if inside actual field boundary, outer boundary is 0 and opposite if (inBndNum == 0 && mf.bndArr[inBndNum].IsPointInsideBoundary(point)) { hlLine.Add(point); } if (inBndNum != 0 && !mf.bndArr[inBndNum].IsPointInsideBoundary(point)) { hlLine.Add(point); } } int headCount = hlLine.Count; //remove the points too close to boundary for (int i = 0; i < ptCount; i++) { for (int j = 0; j < headCount; j++) { //make sure distance between headland and boundary is not less then width distance = glm.Distance(mf.bndArr[inBndNum].bndLine[i], hlLine[j]); if (distance < (totalHeadWidth * 0.98)) { hlLine.RemoveAt(j); headCount = hlLine.Count; j = 0; } } } //fix the gaps and overlaps FixHeadlandList(); //pre calculate all the constants and multiples PreCalcHeadlandLines(); }
public void FixTurnLine(double totalHeadWidth, List <vec3> curBnd, double spacing) { //count the points from the boundary int lineCount = turnLine.Count; double distance = 0; //int headCount = mf.bndArr[inTurnNum].bndLine.Count; int bndCount = curBnd.Count; //remove the points too close to boundary for (int i = 0; i < bndCount; i++) { for (int j = 0; j < lineCount; j++) { //make sure distance between headland and boundary is not less then width distance = glm.Distance(curBnd[i], turnLine[j]); if (distance < (totalHeadWidth * 0.99)) { turnLine.RemoveAt(j); lineCount = turnLine.Count; j = -1; } } } //make sure distance isn't too small between points on turnLine bndCount = turnLine.Count; //double spacing = mf.tool.toolWidth * 0.25; for (int i = 0; i < bndCount - 1; i++) { distance = glm.Distance(turnLine[i], turnLine[i + 1]); if (distance < spacing) { turnLine.RemoveAt(i + 1); bndCount = turnLine.Count; i--; } } //make sure distance isn't too big between points on Turn bndCount = turnLine.Count; for (int i = 0; i < bndCount; i++) { int j = i + 1; if (j == bndCount) { j = 0; } distance = glm.Distance(turnLine[i], turnLine[j]); if (distance > (spacing * 1.25)) { vec3 pointB = new vec3((turnLine[i].easting + turnLine[j].easting) / 2.0, (turnLine[i].northing + turnLine[j].northing) / 2.0, turnLine[i].heading); turnLine.Insert(j, pointB); bndCount = turnLine.Count; i--; } } //make sure headings are correct for calculated points if (turnLine.Count > 0) { CalculateTurnHeadings(); } //int cnt = turnLine.Count; //vec3[] arr = new vec3[cnt]; //turnLine.CopyTo(arr); //turnLine.Clear(); //double delta = 0; //for (int i = 0; i < arr.Length; i++) //{ // if (i == 0) // { // turnLine.Add(new vec3(arr[i].easting, arr[i].northing, arr[i].heading)); // continue; // } // delta += (arr[i - 1].heading - arr[i].heading); // if (Math.Abs(delta) > 0.1) // { // vec3 pt = new vec3(arr[i].easting, arr[i].northing, arr[i].heading); // turnLine.Add(pt); // delta = 0; // } //} }
public void GetCurrentABLine(vec3 pivot) { //move the ABLine over based on the overlap amount set in vehicle double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap; //x2-x1 double dx = refABLineP2.easting - refABLineP1.easting; //z2-z1 double dy = refABLineP2.northing - refABLineP1.northing; //how far are we away from the reference line at 90 degrees distanceFromRefLine = ((dy * pivot.easting) - (dx * pivot.northing) + (refABLineP2.easting * refABLineP1.northing) - (refABLineP2.northing * refABLineP1.easting)) / Math.Sqrt((dy * dy) + (dx * dx)); //sign of distance determines which side of line we are on if (distanceFromRefLine > 0) { refLineSide = 1; } else { refLineSide = -1; } //absolute the distance distanceFromRefLine = Math.Abs(distanceFromRefLine); //Which ABLine is the vehicle on, negative is left and positive is right side howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero); //generate that pass number as signed integer passNumber = Convert.ToInt32(refLineSide * howManyPathsAway); //calculate the new point that is number of implement widths over double toolOffset = mf.vehicle.toolOffset; vec2 point1; //depending which way you are going, the offset can be either side if (isABSameAsVehicleHeading) { point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting, (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing); } else { point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting, (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing); } //create the new line extent points for current ABLine based on original heading of AB line currentABLineP1.easting = point1.easting - (Math.Sin(abHeading) * 40000.0); currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0); currentABLineP2.easting = point1.easting + (Math.Sin(abHeading) * 40000.0); currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0); //get the distance from currently active AB line //x2-x1 dx = currentABLineP2.easting - currentABLineP1.easting; //z2-z1 dy = currentABLineP2.northing - currentABLineP1.northing; //save a copy of dx,dy in youTurn mf.yt.dxAB = dx; mf.yt.dyAB = dy; //how far from current AB Line is fix distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting)) / Math.Sqrt((dy * dy) + (dx * dx)); //are we on the right side or not isOnRightSideCurrentLine = distanceFromCurrentLine > 0; //absolute the distance distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine); //update base on autosteer settings and distance from line double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine); mf.lookaheadActual = goalPointDistance; //Subtract the two headings, if > 1.57 its going the opposite heading as refAB abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading)); if (abFixHeadingDelta >= Math.PI) { abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI); } // ** Pure pursuit ** - calc point on ABLine closest to current position double U = (((pivot.easting - currentABLineP1.easting) * dx) + ((pivot.northing - currentABLineP1.northing) * dy)) / ((dx * dx) + (dy * dy)); //point on AB line closest to pivot axle point rEastAB = currentABLineP1.easting + (U * dx); rNorthAB = currentABLineP1.northing + (U * dy); if (abFixHeadingDelta >= glm.PIBy2) { isABSameAsVehicleHeading = false; goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance); goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance); } else { isABSameAsVehicleHeading = true; goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance); goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance); } //calc "D" the distance from pivot axle to lookahead point double goalPointDistanceDSquared = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting); //calculate the the new x in local coordinates and steering angle degrees based on wheelbase double localHeading = glm.twoPI - mf.fixHeading; ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading)))); steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceDSquared)); if (steerAngleAB < -mf.vehicle.maxSteerAngle) { steerAngleAB = -mf.vehicle.maxSteerAngle; } if (steerAngleAB > mf.vehicle.maxSteerAngle) { steerAngleAB = mf.vehicle.maxSteerAngle; } //limit circle size for display purpose if (ppRadiusAB < -500) { ppRadiusAB = -500; } if (ppRadiusAB > 500) { ppRadiusAB = 500; } radiusPointAB.easting = pivot.easting + (ppRadiusAB * Math.Cos(localHeading)); radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading)); //Convert to millimeters distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero); //angular velocity in rads/sec = 2PI * m/sec * radians/meters angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase; //clamp the steering angle to not exceed safe angular velocity if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity) { steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))) : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))); } //distance is negative if on left, positive if on right if (isABSameAsVehicleHeading) { if (!isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } } //opposite way so right is left else { if (isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } } mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine; mf.guidanceLineSteerAngle = (Int16)(steerAngleAB * 100); if (mf.yt.isYouTurnTriggered) { //do the pure pursuit from youTurn mf.yt.DistanceFromYouTurnLine(); mf.seq.DoSequenceEvent(); //now substitute what it thinks are AB line values with auto turn values steerAngleAB = mf.yt.steerAngleYT; distanceFromCurrentLine = mf.yt.distanceFromCurrentLine; goalPointAB = mf.yt.goalPointYT; radiusPointAB.easting = mf.yt.radiusPointYT.easting; radiusPointAB.northing = mf.yt.radiusPointYT.northing; ppRadiusAB = mf.yt.ppRadiusYT; } }
//Add current position to stripList public void AddPoint(vec3 pivot) { vec3 point = new vec3(pivot.easting, pivot.northing, pivot.heading); ptList.Add(point); }
private void btnMakeBoundaryCurve_Click(object sender, EventArgs e) { //count the points from the boundary int ptCount = mf.bnd.bndList[0].fenceLine.Count; mf.curve.refList?.Clear(); //outside point vec3 pt3 = new vec3(); double moveDist = (double)nudDistance.Value * mf.inchOrCm2m; double distSq = (moveDist) * (moveDist) * 0.999; //make the boundary tram outer array for (int i = 0; i < ptCount; i++) { //calculate the point inside the boundary pt3.easting = mf.bnd.bndList[0].fenceLine[i].easting - (Math.Sin(glm.PIBy2 + mf.bnd.bndList[0].fenceLine[i].heading) * (moveDist)); pt3.northing = mf.bnd.bndList[0].fenceLine[i].northing - (Math.Cos(glm.PIBy2 + mf.bnd.bndList[0].fenceLine[i].heading) * (moveDist)); pt3.heading = mf.bnd.bndList[0].fenceLine[i].heading; bool Add = true; for (int j = 0; j < ptCount; j++) { double check = glm.DistanceSquared(pt3.northing, pt3.easting, mf.bnd.bndList[0].fenceLine[j].northing, mf.bnd.bndList[0].fenceLine[j].easting); if (check < distSq) { Add = false; break; } } if (Add) { if (mf.curve.refList.Count > 0) { double dist = ((pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting) * (pt3.easting - mf.curve.refList[mf.curve.refList.Count - 1].easting)) + ((pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing) * (pt3.northing - mf.curve.refList[mf.curve.refList.Count - 1].northing)); if (dist > 1) { mf.curve.refList.Add(pt3); } } else { mf.curve.refList.Add(pt3); } } } btnCancelTouch.Enabled = false; int cnt = mf.curve.refList.Count; if (cnt > 3) { //make sure distance isn't too big between points on Turn for (int i = 0; i < cnt - 1; i++) { int j = i + 1; //if (j == cnt) j = 0; double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]); if (distance > 1.2) { vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0, (mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0, mf.curve.refList[i].heading); mf.curve.refList.Insert(j, pointB); cnt = mf.curve.refList.Count; i = -1; } } //who knows which way it actually goes mf.curve.CalculateTurnHeadings(); mf.curve.isCurveSet = true; mf.curve.aveLineHeading = 0; //mf.curve.SmoothAB(4); //mf.curve.CalculateTurnHeadings(); mf.curve.isCurveSet = true; //double offset = ((double)nudDistance.Value) / 200.0; mf.curve.curveArr.Add(new CCurveLines()); mf.curve.numCurveLines = mf.curve.curveArr.Count; mf.curve.numCurveLineSelected = mf.curve.numCurveLines; //array number is 1 less since it starts at zero int idx = mf.curve.curveArr.Count - 1; //create a name mf.curve.curveArr[idx].Name = "Boundary Curve"; mf.curve.curveArr[idx].aveHeading = mf.curve.aveLineHeading; //write out the Curve Points foreach (vec3 item in mf.curve.refList) { mf.curve.curveArr[idx].curvePts.Add(item); } mf.FileSaveCurveLines(); //update the arrays btnMakeABLine.Enabled = false; btnMakeCurve.Enabled = false; isMakingCurve = false; isMakingAB = false; start = 99999; end = 99999; FixLabelsCurve(); } else { mf.curve.isCurveSet = false; mf.curve.refList?.Clear(); } btnExit.Focus(); }
//determine closest point on left side public void BuildContourGuidanceLine(vec3 pivot) { //2 triangles EAD and CBF double sinH = Math.Sin(pivot.heading) * 1.5 * mf.vehicle.toolWidth; double cosH = Math.Cos(pivot.heading) * 1.5 * mf.vehicle.toolWidth; double sin2H = Math.Sin(pivot.heading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth; double cos2H = Math.Cos(pivot.heading + glm.PIBy2) * 1.5 * mf.vehicle.toolWidth; double sin3H = Math.Sin(pivot.heading + glm.PIBy2) * 0.5; double cos3H = Math.Cos(pivot.heading + glm.PIBy2) * 0.5; //build a frustum box ahead of fix to find adjacent paths and points boxA.easting = pivot.easting - (sin2H); boxA.northing = pivot.northing - (cos2H); boxA.easting -= (sinH * 0.5); boxA.northing -= (cosH * 0.5); boxB.easting = pivot.easting + (sin2H); boxB.northing = pivot.northing + (cos2H); boxB.easting -= (sinH * 0.5); boxB.northing -= (cosH * 0.5); boxC.easting = boxB.easting + (sinH); boxC.northing = boxB.northing + (cosH); boxD.easting = boxA.easting + (sinH); boxD.northing = boxA.northing + (cosH); boxE.easting = pivot.easting - (sin3H); boxE.northing = pivot.northing - (cos3H); boxF.easting = pivot.easting + (sin3H); boxF.northing = pivot.northing + (cos3H); conList.Clear(); ctList.Clear(); int ptCount; //check if no strips yet, return int stripCount = stripList.Count; if (stripCount == 0) return; cvec pointC = new cvec(); //determine if points are in right side frustum box for (int s = 0; s < stripCount; s++) { ptCount = stripList[s].Count; for (int p = 0; p < ptCount; p++) { if ((((boxF.easting - boxC.easting) * (stripList[s][p].northing - boxC.northing)) - ((boxF.northing - boxC.northing) * (stripList[s][p].easting - boxC.easting))) < 0) { continue; } if ((((boxC.easting - boxB.easting) * (stripList[s][p].northing - boxB.northing)) - ((boxC.northing - boxB.northing) * (stripList[s][p].easting - boxB.easting))) < 0) { continue; } if ((((boxB.easting - boxF.easting) * (stripList[s][p].northing - boxF.northing)) - ((boxB.northing - boxF.northing) * (stripList[s][p].easting - boxF.easting))) < 0) { continue; } //in the box so is it parallelish or perpedicularish to current heading ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI); if (ref2 < 1.2 || ref2 > 1.9) { //it's in the box and parallelish so add to list pointC.x = stripList[s][p].easting; pointC.z = stripList[s][p].northing; pointC.h = stripList[s][p].heading; pointC.strip = s; pointC.pt = p; conList.Add(pointC); } } } if (conList.Count == 0) { //determine if points are in frustum box for (int s = 0; s < stripCount; s++) { ptCount = stripList[s].Count; for (int p = 0; p < ptCount; p++) { if ((((boxE.easting - boxA.easting) * (stripList[s][p].northing - boxA.northing)) - ((boxE.northing - boxA.northing) * (stripList[s][p].easting - boxA.easting))) < 0) { continue; } if ((((boxD.easting - boxE.easting) * (stripList[s][p].northing - boxE.northing)) - ((boxD.northing - boxE.northing) * (stripList[s][p].easting - boxE.easting))) < 0) { continue; } if ((((boxA.easting - boxD.easting) * (stripList[s][p].northing - boxD.northing)) - ((boxA.northing - boxD.northing) * (stripList[s][p].easting - boxD.easting))) < 0) { continue; } //in the box so is it parallelish or perpedicularish to current heading ref2 = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - stripList[s][p].heading) - Math.PI); if (ref2 < 1.2 || ref2 > 1.9) { //it's in the box and parallelish so add to list pointC.x = stripList[s][p].easting; pointC.z = stripList[s][p].northing; pointC.h = stripList[s][p].heading; pointC.strip = s; pointC.pt = p; conList.Add(pointC); } } } } //no points in the box, exit ptCount = conList.Count; if (ptCount == 0) { distanceFromCurrentLine = 9999; distanceFromCurrentLine = 32000; mf.guidanceLineDistanceOff = 32000; return; } //determine closest point minDistance = 99999; for (int i = 0; i < ptCount; i++) { double dist = ((pivot.easting - conList[i].x) * (pivot.easting - conList[i].x)) + ((pivot.northing - conList[i].z) * (pivot.northing - conList[i].z)); if (minDistance >= dist) { minDistance = dist; closestRefPoint = i; } } //now we have closest point, the distance squared from it, and which patch and point its from int strip = conList[closestRefPoint].strip; int pt = conList[closestRefPoint].pt; refX = stripList[strip][pt].easting; refZ = stripList[strip][pt].northing; refHeading = stripList[strip][pt].heading; //are we going same direction as stripList was created? bool isSameWay = Math.PI - Math.Abs(Math.Abs(mf.fixHeading - refHeading) - Math.PI) < 1.4; //which side of the patch are we on is next //calculate endpoints of reference line based on closest point refPoint1.easting = refX - (Math.Sin(refHeading) * 50.0); refPoint1.northing = refZ - (Math.Cos(refHeading) * 50.0); refPoint2.easting = refX + (Math.Sin(refHeading) * 50.0); refPoint2.northing = refZ + (Math.Cos(refHeading) * 50.0); //x2-x1 double dx = refPoint2.easting - refPoint1.easting; //z2-z1 double dz = refPoint2.northing - refPoint1.northing; //how far are we away from the reference line at 90 degrees - 2D cross product and distance distanceFromRefLine = ((dz * mf.pn.fix.easting) - (dx * mf.pn.fix.northing) + (refPoint2.easting * refPoint1.northing) - (refPoint2.northing * refPoint1.easting)) / Math.Sqrt((dz * dz) + (dx * dx)); //add or subtract pi by 2 depending on which side of ref line double piSide; //sign of distance determines which side of line we are on if (distanceFromRefLine > 0) piSide = glm.PIBy2; else piSide = -glm.PIBy2; //offset calcs double toolOffset = mf.vehicle.toolOffset; if (isSameWay) { toolOffset = 0.0; } else { if (distanceFromRefLine > 0) toolOffset *= 2.0; else toolOffset *= -2.0; } //move the Guidance Line over based on the overlap, width, and offset amount set in vehicle double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap + toolOffset; //absolute the distance distanceFromRefLine = Math.Abs(distanceFromRefLine); //make the new guidance line list called guideList ptCount = stripList[strip].Count - 1; int start, stop; if (isSameWay) { start = pt - 35; if (start < 0) start = 0; stop = pt + 40; if (stop > ptCount) stop = ptCount + 1; } else { start = pt - 40; if (start < 0) start = 0; stop = pt + 35; if (stop > ptCount) stop = ptCount + 1; } double distSq = widthMinusOverlap * widthMinusOverlap * 0.98; bool fail = false; for (int i = start; i < stop; i++) { var point = new vec3( stripList[strip][i].easting + (Math.Sin(piSide + stripList[strip][i].heading) * widthMinusOverlap), stripList[strip][i].northing + (Math.Cos(piSide + stripList[strip][i].heading) * widthMinusOverlap), stripList[strip][i].heading); //ctList.Add(point); //make sure its not closer then 1 eq width for (int j = start; j < stop; j++) { double check = glm.DistanceSquared(point.northing, point.easting, stripList[strip][j].northing, stripList[strip][j].easting); if (check < distSq) { fail = true; break; } } if (!fail) ctList.Add(point); fail = false; } }
private void btnLoadBoundaryFromGE_Click(object sender, EventArgs e) { if (sender is Button button) { Selectedreset = true; btnLoadBoundaryFromGE.Enabled = false; btnDelete.Enabled = false; string fileAndDirectory; { //create the dialog instance OpenFileDialog ofd = new OpenFileDialog { //set the filter to text KML only Filter = "KML files (*.KML)|*.KML", //the initial directory, fields, for the open dialog InitialDirectory = mf.fieldsDirectory + mf.currentFieldDirectory }; //was a file selected if (ofd.ShowDialog() == DialogResult.Cancel) { return; } else { fileAndDirectory = ofd.FileName; } } //start to read the file string line = null; string coordinates = null; int startIndex; int i = 0; using (System.IO.StreamReader reader = new System.IO.StreamReader(fileAndDirectory)) { if (button.Name == "btnLoadMultiBoundaryFromGE") { ResetAllBoundary(); } else { i = mf.bnd.boundarySelected; } try { while (!reader.EndOfStream) { line = reader.ReadLine(); startIndex = line.IndexOf("<coordinates>"); if (startIndex != -1) { while (true) { int endIndex = line.IndexOf("</coordinates>"); if (endIndex == -1) { //just add the line if (startIndex == -1) { coordinates += line.Substring(0); } else { coordinates += line.Substring(startIndex + 13); } } else { if (startIndex == -1) { coordinates += line.Substring(0, endIndex); } else { coordinates += line.Substring(startIndex + 13, endIndex - (startIndex + 13)); } break; } line = reader.ReadLine(); line = line.Trim(); startIndex = -1; } line = coordinates; char[] delimiterChars = { ' ', '\t', '\r', '\n' }; string[] numberSets = line.Split(delimiterChars); //at least 3 points if (numberSets.Length > 2) { mf.bnd.bndArr.Add(new CBoundaryLines()); mf.turn.turnArr.Add(new CTurnLines()); mf.gf.geoFenceArr.Add(new CGeoFenceLines()); foreach (var item in numberSets) { string[] fix = item.Split(','); double.TryParse(fix[0], NumberStyles.Float, CultureInfo.InvariantCulture, out lonK); double.TryParse(fix[1], NumberStyles.Float, CultureInfo.InvariantCulture, out latK); double[] xy = mf.pn.DecDeg2UTM(latK, lonK); //match new fix to current position easting = xy[0] - mf.pn.utmEast; northing = xy[1] - mf.pn.utmNorth; double east = easting; double nort = northing; //fix the azimuth error easting = (Math.Cos(-mf.pn.convergenceAngle) * east) - (Math.Sin(-mf.pn.convergenceAngle) * nort); northing = (Math.Sin(-mf.pn.convergenceAngle) * east) + (Math.Cos(-mf.pn.convergenceAngle) * nort); //add the point to boundary vec3 bndPt = new vec3(easting, northing, 0); mf.bnd.bndArr[i].bndLine.Add(bndPt); } //fix the points if there are gaps bigger then mf.bnd.bndArr[i].CalculateBoundaryHeadings(); mf.bnd.bndArr[i].PreCalcBoundaryLines(); mf.bnd.bndArr[i].FixBoundaryLine(i, mf.tool.toolWidth); //boundary area, pre calcs etc mf.bnd.bndArr[i].CalculateBoundaryArea(); mf.bnd.bndArr[i].PreCalcBoundaryLines(); mf.bnd.bndArr[i].isSet = true; //if (i == 0) mf.bnd.bndArr[i].isOwnField = true; //else mf.bnd.bndArr[i].isOwnField = false; coordinates = ""; i++; } else { mf.TimedMessageBox(2000, gStr.gsErrorreadingKML, gStr.gsChooseBuildDifferentone); } if (button.Name == "btnLoadBoundaryFromGE") { break; } } } mf.FileSaveBoundary(); mf.fd.UpdateFieldBoundaryGUIAreas(); UpdateChart(); } catch (Exception) { return; } } } }
//determine distance from contour guidance line public void DistanceFromContourLine(vec3 pivot) { isValid = false; double minDistA = 1000000, minDistB = 1000000; int ptCount = ctList.Count; //distanceFromCurrentLine = 9999; if (ptCount > 0) { //find the closest 2 points to current fix for (int t = 0; t < ptCount; t++) { double dist = ((pivot.easting - ctList[t].easting) * (pivot.easting - ctList[t].easting)) + ((pivot.northing - ctList[t].northing) * (pivot.northing - ctList[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; } //get the distance from currently active AB line //x2-x1 double dx = ctList[B].easting - ctList[A].easting; //z2-z1 double dy = ctList[B].northing - ctList[A].northing; if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dy) < Double.Epsilon) return; //abHeading = Math.Atan2(dz, dx); abHeading = ctList[A].heading; //how far from current AB Line is fix distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (ctList[B].easting * ctList[A].northing) - (ctList[B].northing * ctList[A].easting)) / Math.Sqrt((dy * dy) + (dx * dx)); //are we on the right side or not isOnRightSideCurrentLine = distanceFromCurrentLine > 0; //absolute the distance distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine); // ** Pure pursuit ** - calc point on ABLine closest to current position double U = (((pivot.easting - ctList[A].easting) * (dx)) + ((pivot.northing - ctList[A].northing) * (dy))) / ((dx * dx) + (dy * dy)); rEastCT = ctList[A].easting + (U * (dx)); rNorthCT = ctList[A].northing + (U * (dy)); ////determine if the point is between 2 points initially determined //double minx, maxx, miny, maxy; //minx = Math.Min(ctList[A].northing, ctList[B].northing); //maxx = Math.Max(ctList[A].northing, ctList[B].northing); //miny = Math.Min(ctList[A].easting, ctList[B].easting); //maxy = Math.Max(ctList[A].easting, ctList[B].easting); //isValid = (rNorthCT >= minx && rNorthCT <= maxx) && (rEastCT >= miny && rEastCT <= maxy); //if (!isValid) //{ // //invalid distance so tell AS module // distanceFromCurrentLine = 32000; // mf.guidanceLineDistanceOff = 32000; // return; //} //Subtract the two headings, if > 1.57 its going the opposite heading as refAB abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading)); if (abFixHeadingDelta >= Math.PI) abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI); //used for accumulating distance to find goal point double distSoFar; //how far should goal point be away - speed * seconds * kmph -> m/s + min value double goalPointDistance = mf.pn.speed * mf.vehicle.goalPointLookAhead * 0.27777777; //minimum of 4.0 meters look ahead if (goalPointDistance < 4.0) goalPointDistance = 4.0; // used for calculating the length squared of next segment. double tempDist = 0.0; if (abFixHeadingDelta >= glm.PIBy2) { //counting down isABSameAsFixHeading = false; distSoFar = glm.Distance(ctList[A], rEastCT, rNorthCT); //Is this segment long enough to contain the full lookahead distance? if (distSoFar > goalPointDistance) { //treat current segment like an AB Line goalPointCT.easting = rEastCT - (Math.Sin(ctList[A].heading) * goalPointDistance); goalPointCT.northing = rNorthCT - (Math.Cos(ctList[A].heading) * goalPointDistance); } //multiple segments required else { //cycle thru segments and keep adding lengths. check if start and break if so. while (A > 0) { B--; A--; tempDist = glm.Distance(ctList[B], ctList[A]); //will we go too far? if ((tempDist + distSoFar) > goalPointDistance) { //A++; B++; break; //tempDist contains the full length of next segment } else { distSoFar += tempDist; } } double t = (goalPointDistance - distSoFar); // the remainder to yet travel t /= tempDist; goalPointCT.easting = (((1 - t) * ctList[B].easting) + (t * ctList[A].easting)); goalPointCT.northing = (((1 - t) * ctList[B].northing) + (t * ctList[A].northing)); } } else { //counting up isABSameAsFixHeading = true; distSoFar = glm.Distance(ctList[B], rEastCT, rNorthCT); //Is this segment long enough to contain the full lookahead distance? if (distSoFar > goalPointDistance) { //treat current segment like an AB Line goalPointCT.easting = rEastCT + (Math.Sin(ctList[A].heading) * goalPointDistance); goalPointCT.northing = rNorthCT + (Math.Cos(ctList[A].heading) * goalPointDistance); } //multiple segments required else { //cycle thru segments and keep adding lengths. check if end and break if so. // ReSharper disable once LoopVariableIsNeverChangedInsideLoop while (B < ptCount - 1) { B++; A++; tempDist = glm.Distance(ctList[B], ctList[A]); //will we go too far? if ((tempDist + distSoFar) > goalPointDistance) { //A--; B--; break; //tempDist contains the full length of next segment } distSoFar += tempDist; } //xt = (((1 - t) * x0 + t * x1) //yt = ((1 - t) * y0 + t * y1)) double t = (goalPointDistance - distSoFar); // the remainder to yet travel t /= tempDist; goalPointCT.easting = (((1 - t) * ctList[A].easting) + (t * ctList[B].easting)); goalPointCT.northing = (((1 - t) * ctList[A].northing) + (t * ctList[B].northing)); } } //calc "D" the distance from pivot axle to lookahead point double goalPointDistanceSquared = glm.DistanceSquared(goalPointCT.northing, goalPointCT.easting, pivot.northing, pivot.easting); //calculate the the delta x in local coordinates and steering angle degrees based on wheelbase double localHeading = glm.twoPI - mf.fixHeading; ppRadiusCT = goalPointDistanceSquared / (2 * (((goalPointCT.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCT.northing - pivot.northing) * Math.Sin(localHeading)))); steerAngleCT = glm.toDegrees(Math.Atan(2 * (((goalPointCT.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCT.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared)); if (steerAngleCT < -mf.vehicle.maxSteerAngle) steerAngleCT = -mf.vehicle.maxSteerAngle; if (steerAngleCT > mf.vehicle.maxSteerAngle) steerAngleCT = mf.vehicle.maxSteerAngle; if (ppRadiusCT < -500) ppRadiusCT = -500; if (ppRadiusCT > 500) ppRadiusCT = 500; radiusPointCT.easting = pivot.easting + (ppRadiusCT * Math.Cos(localHeading)); radiusPointCT.northing = pivot.northing + (ppRadiusCT * 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(steerAngleCT))) / mf.vehicle.wheelbase; //clamp the steering angle to not exceed safe angular velocity if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity) { steerAngleCT = glm.toDegrees(steerAngleCT > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))) : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))); } //Convert to centimeters distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero); //distance is negative if on left, positive if on right //if you're going the opposite direction left is right and right is left //double temp; if (isABSameAsFixHeading) { if (!isOnRightSideCurrentLine) distanceFromCurrentLine *= -1.0; } //opposite way so right is left else if (isOnRightSideCurrentLine) { distanceFromCurrentLine *= -1.0; } //fill in the autosteer variables mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine; mf.guidanceLineSteerAngle = (Int16)(steerAngleCT * 100); } else { //invalid distance so tell AS module distanceFromCurrentLine = 32000; mf.guidanceLineDistanceOff = 32000; } }
public void FixBoundaryLine(int bndNum ) { double spacing; //boundary point spacing based on eq width //close if less then 30 ha, 60ha, more then 60 if (area < 200000) spacing = 1.1; else if (area < 400000) spacing = 2.2; else spacing = 3.3; if (bndNum > 0) spacing *= 0.5; int bndCount = bndLine.Count; double distance; //make sure distance isn't too big between points on boundary for (int i = 0; i < bndCount; i++) { int j = i + 1; if (j == bndCount) j = 0; distance = glm.Distance(bndLine[i], bndLine[j]); if (distance > spacing*1.5) { vec3 pointB = new vec3((bndLine[i].easting + bndLine[j].easting) / 2.0, (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading); bndLine.Insert(j, pointB); bndCount = bndLine.Count; i--; } } //make sure distance isn't too big between points on boundary bndCount = bndLine.Count; for (int i = 0; i < bndCount; i++) { int j = i + 1; if (j == bndCount) j = 0; distance = glm.Distance(bndLine[i], bndLine[j]); if (distance > spacing*1.6) { vec3 pointB = new vec3((bndLine[i].easting + bndLine[j].easting) / 2.0, (bndLine[i].northing + bndLine[j].northing) / 2.0, bndLine[i].heading); bndLine.Insert(j, pointB); bndCount = bndLine.Count; i--; } } //make sure distance isn't too small between points on headland spacing *= 1.2; bndCount = bndLine.Count; for (int i = 0; i < bndCount - 1; i++) { distance = glm.Distance(bndLine[i], bndLine[i + 1]); if (distance < spacing) { bndLine.RemoveAt(i + 1); bndCount = bndLine.Count; i--; } } //make sure headings are correct for calculated points CalculateBoundaryHeadings(); double delta = 0; bndLineEar?.Clear(); for (int i = 0; i < bndLine.Count; i++) { if (i == 0) { bndLineEar.Add(new vec2(bndLine[i].easting, bndLine[i].northing)); continue; } delta += (bndLine[i - 1].heading - bndLine[i].heading); if (Math.Abs(delta) > 0.01) { bndLineEar.Add(new vec2(bndLine[i].easting, bndLine[i].northing)); delta = 0; } } }
public void GetCurrentABLine(vec3 pivot, vec3 steer) { double dx, dy; //build new current ref line if required if (!isABValid || ((mf.secondsSinceStart - lastSecond) > 0.66 && (!mf.isAutoSteerBtnOn || mf.mc.steerSwitchHigh))) { BuildCurrentABLineList(pivot); } //Check uturn first if (mf.yt.isYouTurnTriggered && mf.yt.DistanceFromYouTurnLine())//do the pure pursuit from youTurn { //now substitute what it thinks are AB line values with auto turn values steerAngleAB = mf.yt.steerAngleYT; distanceFromCurrentLinePivot = mf.yt.distanceFromCurrentLine; goalPointAB = mf.yt.goalPointYT; radiusPointAB.easting = mf.yt.radiusPointYT.easting; radiusPointAB.northing = mf.yt.radiusPointYT.northing; ppRadiusAB = mf.yt.ppRadiusYT; } //Stanley else if (mf.isStanleyUsed) { mf.gyd.StanleyGuidanceABLine(currentABLineP1, currentABLineP2, pivot, steer); } //Pure Pursuit else { //get the distance from currently active AB line //x2-x1 dx = currentABLineP2.easting - currentABLineP1.easting; //z2-z1 dy = currentABLineP2.northing - currentABLineP1.northing; //save a copy of dx,dy in youTurn mf.yt.dxAB = dx; mf.yt.dyAB = dy; //how far from current AB Line is fix distanceFromCurrentLinePivot = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting)) / Math.Sqrt((dy * dy) + (dx * dx)); //integral slider is set to 0 if (mf.vehicle.purePursuitIntegralGain != 0 && !mf.isReverse) { 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) //&& 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; } //Subtract the two headings, if > 1.57 its going the opposite heading as refAB abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading)); if (abFixHeadingDelta >= Math.PI) { abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI); } // ** Pure pursuit ** - calc point on ABLine closest to current position double U = (((pivot.easting - currentABLineP1.easting) * dx) + ((pivot.northing - currentABLineP1.northing) * dy)) / ((dx * dx) + (dy * dy)); //point on AB line closest to pivot axle point rEastAB = currentABLineP1.easting + (U * dx); rNorthAB = currentABLineP1.northing + (U * dy); //update base on autosteer settings and distance from line double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(); if (mf.isReverse ? isHeadingSameWay : !isHeadingSameWay) { goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance); goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance); } else { goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance); goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance); } //calc "D" the distance from pivot axle to lookahead point double goalPointDistanceDSquared = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting); //calculate the the new x in local coordinates and steering angle degrees based on wheelbase double localHeading; if (isHeadingSameWay) { localHeading = glm.twoPI - mf.fixHeading + inty; } else { localHeading = glm.twoPI - mf.fixHeading - inty; } ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading)))); steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceDSquared)); if (mf.ahrs.imuRoll != 88888) { steerAngleAB += mf.ahrs.imuRoll * -mf.gyd.sideHillCompFactor; } if (steerAngleAB < -mf.vehicle.maxSteerAngle) { steerAngleAB = -mf.vehicle.maxSteerAngle; } if (steerAngleAB > mf.vehicle.maxSteerAngle) { steerAngleAB = mf.vehicle.maxSteerAngle; } //limit circle size for display purpose if (ppRadiusAB < -500) { ppRadiusAB = -500; } if (ppRadiusAB > 500) { ppRadiusAB = 500; } radiusPointAB.easting = pivot.easting + (ppRadiusAB * Math.Cos(localHeading)); radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading)); if (mf.isAngVelGuidance) { //angular velocity in rads/sec = 2PI * m/sec * radians/meters mf.setAngVel = 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase; mf.setAngVel = glm.toDegrees(mf.setAngVel) * 100; //clamp the steering angle to not exceed safe angular velocity if (Math.Abs(mf.setAngVel) > 1000) { //mf.setAngVel = mf.setAngVel < 0 ? -mf.vehicle.maxAngularVelocity : mf.vehicle.maxAngularVelocity; mf.setAngVel = mf.setAngVel < 0 ? -1000 : 1000; } } //distance is negative if on left, positive if on right if (!isHeadingSameWay) { distanceFromCurrentLinePivot *= -1.0; } //Convert to millimeters mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero); mf.guidanceLineSteerAngle = (short)(steerAngleAB * 100); } }
public void FindPointsDriveAround(vec3 fromPt, double headAB, ref vec3 start, ref vec3 stop) { //initial scan is straight ahead of pivot point of vehicle to find the right turnLine/boundary vec3 pt = new vec3(); bool isFound = false; int closestTurnNum = 99; int closestTurnIndex = 99999; int mazeDim = mf.mazeGrid.mazeColXDim * mf.mazeGrid.mazeRowYDim; double cosHead = Math.Cos(headAB); double sinHead = Math.Sin(headAB); for (int b = 1; b < mf.maxCrossFieldLength; b += 2) { pt.easting = fromPt.easting + (sinHead * b); pt.northing = fromPt.northing + (cosHead * b); if (mf.turn.turnArr[0].IsPointInTurnWorkArea(pt)) { for (int t = 1; t < mf.bnd.bndArr.Count; t++) { if (!mf.bnd.bndArr[t].isSet || mf.bnd.bndArr[t].isDriveThru) { continue; } if (mf.bnd.bndArr[t].isDriveAround) { if (mf.gf.geoFenceArr[t].IsPointInGeoFenceArea(pt)) { isFound = true; closestTurnNum = t; closestTurnIndex = b; start.easting = fromPt.easting + (sinHead * b); start.northing = fromPt.northing + (cosHead * b); start.heading = headAB; break; } } else { //its a uturn obstacle if (mf.turn.turnArr[t].IsPointInTurnWorkArea(pt)) { start.easting = 88888; return; } } } } else { isFound = true; start.easting = 88888; closestTurnNum = 0; return; } if (isFound) { break; } } isFound = false; for (int b = closestTurnIndex + 200; b > closestTurnIndex; b--) { pt.easting = fromPt.easting + (sinHead * b); pt.northing = fromPt.northing + (cosHead * b); if (mf.gf.geoFenceArr[closestTurnNum].IsPointInGeoFenceArea(pt)) { isFound = true; stop.easting = fromPt.easting + (sinHead * b); stop.northing = fromPt.northing + (cosHead * b); stop.heading = headAB; } if (isFound) { break; } } for (int i = 0; i < 30; i++) { start.easting -= sinHead; start.northing -= cosHead; start.heading = headAB; int iStart = (int)((((int)((start.northing - mf.minFieldY) / mf.mazeGrid.mazeScale)) * mf.mazeGrid.mazeColXDim) + (int)((start.easting - mf.minFieldX) / mf.mazeGrid.mazeScale)); if (iStart >= mazeDim) { isFound = true; start.easting = 88888; closestTurnNum = 0; return; } if (mf.mazeGrid.mazeArr[iStart] == 0) { break; } } for (int i = 0; i < 30; i++) { stop.easting += sinHead; stop.northing += cosHead; stop.heading = headAB; int iStop = (int)((((int)((stop.northing - mf.minFieldY) / mf.mazeGrid.mazeScale)) * mf.mazeGrid.mazeColXDim) + (int)((stop.easting - mf.minFieldX) / mf.mazeGrid.mazeScale)); if (iStop >= mazeDim) { isFound = true; start.easting = 88888; closestTurnNum = 0; return; } if (mf.mazeGrid.mazeArr[iStop] == 0) { break; } } }
private void btnBPoint_Click(object sender, System.EventArgs e) { mf.curve.aveLineHeading = 0; mf.curve.isOkToAddPoints = false; btnBPoint.Enabled = false; btnAPoint.Enabled = false; btnPausePlay.Enabled = false; btnCancel.Enabled = false; lvLines.Enabled = false; int cnt = mf.curve.refList.Count; if (cnt > 3) { //make sure distance isn't too big between points on Turn for (int i = 0; i < cnt - 1; i++) { int j = i + 1; //if (j == cnt) j = 0; double distance = glm.Distance(mf.curve.refList[i], mf.curve.refList[j]); if (distance > 1.2) { vec3 pointB = new vec3((mf.curve.refList[i].easting + mf.curve.refList[j].easting) / 2.0, (mf.curve.refList[i].northing + mf.curve.refList[j].northing) / 2.0, mf.curve.refList[i].heading); mf.curve.refList.Insert(j, pointB); cnt = mf.curve.refList.Count; i = -1; } } //calculate average heading of line double x = 0, y = 0; mf.curve.isCurveSet = true; foreach (var pt in mf.curve.refList) { x += Math.Cos(pt.heading); y += Math.Sin(pt.heading); } x /= mf.curve.refList.Count; y /= mf.curve.refList.Count; mf.curve.aveLineHeading = Math.Atan2(y, x); if (mf.curve.aveLineHeading < 0) { mf.curve.aveLineHeading += glm.twoPI; } //build the tail extensions mf.curve.AddFirstLastPoints(); SmoothAB(4); mf.curve.CalculateTurnHeadings(); mf.curve.isCurveSet = true; mf.EnableYouTurnButtons(); //mf.FileSaveCurveLine(); lblCurveExists.Text = gStr.gsCurveSet; ShowSavedPanel(true); btnAddAndGo.Enabled = true; btnAddToFile.Enabled = true; btnAPoint.Enabled = false; btnBPoint.Enabled = false; btnPausePlay.Enabled = false; textBox1.BackColor = Color.LightGreen; textBox1.Enabled = true; textBox1.Text = (Math.Round(glm.toDegrees(mf.curve.aveLineHeading), 1)).ToString(CultureInfo.InvariantCulture) + "\u00B0" + mf.FindDirection(mf.curve.aveLineHeading) + DateTime.Now.ToString("hh:mm:ss", CultureInfo.InvariantCulture); } else { mf.curve.isCurveSet = false; mf.curve.refList?.Clear(); lblCurveExists.Text = " > Off <"; ShowSavedPanel(true); btnNewCurve.Enabled = true; btnCancel.Enabled = true; lvLines.Enabled = true; } lvLines.SelectedItems.Clear(); }