From c5eaccf00c80f1ce77c3e411cfde4798be093c16 Mon Sep 17 00:00:00 2001 From: Brian Tischler Date: Mon, 4 Jul 2022 20:16:09 -0600 Subject: [PATCH 1/3] offset fix --- SourceCode/GPS/Classes/CContour.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/SourceCode/GPS/Classes/CContour.cs b/SourceCode/GPS/Classes/CContour.cs index bf1d9d307..f74efe4d8 100644 --- a/SourceCode/GPS/Classes/CContour.cs +++ b/SourceCode/GPS/Classes/CContour.cs @@ -844,7 +844,9 @@ public void StartContourLine(vec3 pivot) //Add current position to stripList public void AddPoint(vec3 pivot) { - ptList.Add(new vec3(pivot.easting, pivot.northing, pivot.heading)); + ptList.Add(new vec3(pivot.easting + Math.Cos(pivot.heading) * mf.tool.toolOffset, + pivot.northing - Math.Sin(pivot.heading) * mf.tool.toolOffset, + pivot.heading)); } //End the strip From eacca03503731c06bfe600dd4f15293d4ed37ca8 Mon Sep 17 00:00:00 2001 From: Brian Tischler Date: Tue, 5 Jul 2022 09:15:18 -0600 Subject: [PATCH 2/3] More contour fixes. --- SourceCode/GPS/Classes/CContour.cs | 52 ++++++++++------------- SourceCode/GPS/Forms/Position.designer.cs | 52 +++++++++++------------ 2 files changed, 48 insertions(+), 56 deletions(-) diff --git a/SourceCode/GPS/Classes/CContour.cs b/SourceCode/GPS/Classes/CContour.cs index f74efe4d8..6be860ad9 100644 --- a/SourceCode/GPS/Classes/CContour.cs +++ b/SourceCode/GPS/Classes/CContour.cs @@ -16,7 +16,7 @@ public class CContour public double distanceFromCurrentLinePivot; - private int A, B, C, stripNum, lastLockPt = int.MaxValue, backSpacing = 30; + private int A, B, C, stripNum, lastLockPt = int.MaxValue; public double abFixHeadingDelta, abHeading; @@ -285,6 +285,7 @@ public void SetLockToLine() //} #endregion private double lastSecond; + int pt = 0; public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) { if (ctList.Count == 0) @@ -293,7 +294,7 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) } else { - if ((mf.secondsSinceStart - lastSecond) < 1.5) return; + if ((mf.secondsSinceStart - lastSecond) < 2) return; } lastSecond = mf.secondsSinceStart; @@ -303,7 +304,6 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) double toolContourDistance = (mf.tool.toolWidth * 3 + Math.Abs(mf.tool.toolOffset)); - int pt = 0; //check if no strips yet, return int stripCount = stripList.Count; @@ -316,8 +316,8 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) double cosH = Math.Cos(pivot.heading) * 0.2; - double sin2HL = Math.Sin(pivot.heading + glm.PIBy2) * 1; - double cos2HL = Math.Cos(pivot.heading + glm.PIBy2) * 1; + double sin2HL = Math.Sin(pivot.heading + glm.PIBy2); + double cos2HL = Math.Cos(pivot.heading + glm.PIBy2); boxA.easting = pivot.easting - sin2HL+ sinH; boxA.northing = pivot.northing - cos2HL+cosH; @@ -335,7 +335,7 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) ptCount = stripList[s].Count; if (ptCount == 0) continue; double dist; - for (p = 0; p < ptCount; p += 6) + for (p = 0; p < ptCount; p += 3) { //if (s == stripCount - 1) { @@ -374,9 +374,6 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) //no points in the box, exit ptCount = stripList[stripNum].Count; - start = lastLockPt - 20; if (start < 0) start = 0; - stop = lastLockPt + 20; if (stop > ptCount) stop = ptCount; - if (ptCount < 2 ) { ctList.Clear(); @@ -384,13 +381,16 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) return; } + start = lastLockPt - 20; if (start < 0) start = 0; + stop = lastLockPt + 20; if (stop > ptCount) stop = ptCount; + //determine closest point minDistance = double.MaxValue; //if being built, start high, keep from guiding latest points made //int currentStripBox = 0; //if (stripNum == stripCount) currentStripBox = 10; - for (int i = start; i < stop; i++) + for (int i = start; i < stop; i+=3) { double dist = ((pivot.easting - stripList[stripNum][i].easting) * (pivot.easting - stripList[stripNum][i].easting)) + ((pivot.northing - stripList[stripNum][i].northing) * (pivot.northing - stripList[stripNum][i].northing)); @@ -449,7 +449,8 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) double howManyPathsAway = 0; - if (Math.Abs(distanceFromRefLine) > mf.tool.halfToolWidth) + if (Math.Abs(distanceFromRefLine) > mf.tool.halfToolWidth + || Math.Abs(mf.tool.toolOffset) > mf.tool.halfToolWidth) { //beside what is done if (RefDist < 0) howManyPathsAway = -1; @@ -457,17 +458,8 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) } else { - if (Math.Abs(mf.tool.toolOffset) > mf.tool.halfToolWidth) - { - if (RefDist < 0) howManyPathsAway = -1; - else howManyPathsAway = 1; - } - - else - { - //driving on what is done - howManyPathsAway = 0; - } + //driving on what is done + howManyPathsAway = 0; } if (howManyPathsAway >= -1 && howManyPathsAway <= 1) @@ -475,8 +467,8 @@ public void BuildContourGuidanceLine(vec3 pivot, vec3 steer) ctList.Clear(); //don't guide behind yourself - if (stripNum == stripList.Count-1 && howManyPathsAway == 0) - return; + //if (stripNum == stripList.Count-1 && howManyPathsAway == 0) + //return; //make the new guidance line list called guideList ptCount = stripList[stripNum].Count; @@ -977,7 +969,7 @@ public void DrawContourLine() //Draw the captured ref strip, red if locked if (isLocked) { - GL.Color3(0.983f, 0.2f, 0.20f); + GL.Color3(0.983f, 0.92f, 0.420f); GL.LineWidth(4); } else @@ -1036,11 +1028,11 @@ public void DrawContourLine() // for (int i = 0; i < ptCount; i++) GL.Vertex3(conList[i].x, conList[i].z, 0); // GL.End(); - // GL.Color3(0.35f, 0.30f, 0.90f); - // GL.PointSize(6.0f); - // GL.Begin(PrimitiveType.Points); - // GL.Vertex3(conList[closestRefPoint].x, conList[closestRefPoint].z, 0); - // GL.End(); + GL.Color3(0.35f, 0.30f, 0.90f); + GL.PointSize(6.0f); + GL.Begin(PrimitiveType.Points); + GL.Vertex3(stripList[stripNum][pt].easting, stripList[stripNum][pt].northing, 0); + GL.End(); //} if (mf.isPureDisplayOn && distanceFromCurrentLinePivot != 32000 && !mf.isStanleyUsed) diff --git a/SourceCode/GPS/Forms/Position.designer.cs b/SourceCode/GPS/Forms/Position.designer.cs index 884b39f63..4ba1a3c8b 100644 --- a/SourceCode/GPS/Forms/Position.designer.cs +++ b/SourceCode/GPS/Forms/Position.designer.cs @@ -362,34 +362,34 @@ public void UpdateFixPosition() if (distanceCurrentStepFix > minFixStepDist) { //most recent heading - double newHeading = Math.Atan2(pn.fix.easting - lastGPS.easting, - pn.fix.northing - lastGPS.northing); + //double newHeading = Math.Atan2(pn.fix.easting - lastGPS.easting, + //pn.fix.northing - lastGPS.northing); //Pointing the opposite way the fixes are moving //if (vehicle.isReverse) gpsHeading += Math.PI; - if (newHeading < 0) newHeading += glm.twoPI; - - if (ahrs.isReverseOn) - { - //what is angle between the last valid heading before stopping and one just now - double delta = Math.Abs(Math.PI - Math.Abs(Math.Abs(newHeading - gpsHeading) - Math.PI)); - - //ie change in direction - { - if (delta > 1.57) // - { - isReverse = true; - newHeading += Math.PI; - if (newHeading < 0) newHeading += glm.twoPI; - if (newHeading >= glm.twoPI) newHeading -= glm.twoPI; - } - else - isReverse = false; - } - } + //if (newHeading < 0) newHeading += glm.twoPI; + + //if (ahrs.isReverseOn) + //{ + // //what is angle between the last valid heading before stopping and one just now + // double delta = Math.Abs(Math.PI - Math.Abs(Math.Abs(newHeading - gpsHeading) - Math.PI)); + + // //ie change in direction + // { + // if (delta > 1.57) // + // { + // isReverse = true; + // newHeading += Math.PI; + // if (newHeading < 0) newHeading += glm.twoPI; + // if (newHeading >= glm.twoPI) newHeading -= glm.twoPI; + // } + // else + // isReverse = false; + // } + //} //set the headings - fixHeading = gpsHeading = newHeading; + //fixHeading = gpsHeading = newHeading; lastGPS.easting = pn.fix.easting; lastGPS.northing = pn.fix.northing; @@ -595,6 +595,9 @@ public void UpdateFixPosition() pn.fix.northing = (Math.Sin(-gpsHeading) * rollCorrectionDistance) + pn.fix.northing; } + //grab the most current fix and save the distance from the last fix + distanceCurrentStepFix = glm.Distance(pn.fix, prevFix); + if (glm.DistanceSquared(lastReverseFix, pn.fix) > 0.5) { //most recent heading @@ -611,9 +614,6 @@ public void UpdateFixPosition() lastReverseFix = pn.fix; } - //grab the most current fix and save the distance from the last fix - distanceCurrentStepFix = glm.Distance(pn.fix, prevFix); - double camDelta = fixHeading - smoothCamHeading; if (camDelta < 0) camDelta += glm.twoPI; From ea909b4954746a56b554890d053ce70a4fe1ce2d Mon Sep 17 00:00:00 2001 From: Brian Tischler Date: Thu, 7 Jul 2022 08:37:37 -0600 Subject: [PATCH 3/3] AgIO steer connect --- SourceCode/AgIO/Source/Forms/FormLoop.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SourceCode/AgIO/Source/Forms/FormLoop.cs b/SourceCode/AgIO/Source/Forms/FormLoop.cs index 778c0b660..0549dec3c 100644 --- a/SourceCode/AgIO/Source/Forms/FormLoop.cs +++ b/SourceCode/AgIO/Source/Forms/FormLoop.cs @@ -279,7 +279,7 @@ private void FormLoop_FormClosing(object sender, FormClosingEventArgs e) { Settings.Default.setPort_wasGPSConnected = wasGPSConnectedLastRun; Settings.Default.setPort_wasIMUConnected = wasIMUConnectedLastRun; - Settings.Default.setPort_wasSteerModuleConnected = wasRtcmConnectedLastRun; + Settings.Default.setPort_wasSteerModuleConnected = wasSteerModuleConnectedLastRun; Settings.Default.setPort_wasMachineModuleConnected = wasMachineModuleConnectedLastRun; Settings.Default.setPort_wasRtcmConnected = wasRtcmConnectedLastRun;