//calculate the extreme tool left, right velocities, each section lookahead, and whether or not its going backwards
public void CalculateSectionLookAhead(double northing, double easting, double cosHeading, double sinHeading)
{
//calculate left side of section 1
vec2 left = new vec2(0, 0);
vec2 right = left;
double leftSpeed = 0, rightSpeed = 0, leftLook = 0, rightLook = 0;
//now loop all the section rights and the one extreme left
for (int j = 0; j < vehicle.numOfSections; j++)
{
if (j == 0)
{
//only one first left point, the rest are all rights moved over to left
section[j].leftPoint = new vec2(cosHeading * (section[j].positionLeft) + easting,
sinHeading * (section[j].positionLeft) + northing);
left = section[j].leftPoint - section[j].lastLeftPoint;
//save a copy for next time
section[j].lastLeftPoint = section[j].leftPoint;
//get the speed for left side only once
leftSpeed = left.GetLength() / fixUpdateTime * 10;
leftLook = leftSpeed * vehicle.toolLookAhead;
//save the far left speed
vehicle.toolFarLeftSpeed = leftSpeed;
}
else
{
//right point from last section becomes this left one
section[j].leftPoint = section[j-1].rightPoint;
left = section[j].leftPoint - section[j].lastLeftPoint;
//save a copy for next time
section[j].lastLeftPoint = section[j].leftPoint;
leftSpeed = rightSpeed;
}
section[j].rightPoint = new vec2(cosHeading * (section[j].positionRight) + easting,
sinHeading * (section[j].positionRight) + northing);
//now we have left and right for this section
right = section[j].rightPoint - section[j].lastRightPoint;
//save a copy for next time
section[j].lastRightPoint = section[j].rightPoint;
//grab vector length and convert to meters/sec/10 pixels per meter
rightSpeed = right.GetLength() / fixUpdateTime * 10;
rightLook = rightSpeed * vehicle.toolLookAhead;
//choose fastest speed
if (leftLook > rightLook) section[j].sectionLookAhead = leftLook;
else section[j].sectionLookAhead = rightLook;
if (section[j].sectionLookAhead > 190) section[j].sectionLookAhead = 190;
if (section[j].sectionLookAhead < 5) section[j].sectionLookAhead = 5;
//Doing the screen jump thing, exceeding buffer so just set as minimum
if (currentStepFix >= totalFixSteps - 1) section[j].sectionLookAhead = 5;
//Reverse section turnoff
//double head = left.HeadingXZ();
//if (head < 0) head += glm.twoPI;
//double head2 = right.HeadingXZ();
//if (head2 < 0) head2 += glm.twoPI;
////if both left and right sides of section are going backwards turn off section, negative lookahead
//if (Math.PI - Math.Abs(Math.Abs(head - fixHeadingSection) - Math.PI) > glm.PIBy2 &&
// Math.PI - Math.Abs(Math.Abs(head2 - fixHeadingSection) - Math.PI) > glm.PIBy2)
// section[j].sectionLookAhead = Math.Abs(section[j].sectionLookAhead) * -1;
}//endfor
//with left and right tool velocity to determine rate of triangle generation, corners are more
//save far right speed, 0 if going backwards, in meters/sec
if (section[vehicle.numOfSections - 1].sectionLookAhead > 0) vehicle.toolFarRightSpeed = rightSpeed * 0.05;
else vehicle.toolFarRightSpeed = 0;
//safe left side, 0 if going backwards, in meters/sec convert back from pixels/m
if (section[0].sectionLookAhead > 0) vehicle.toolFarLeftSpeed = vehicle.toolFarLeftSpeed * 0.05;
else vehicle.toolFarLeftSpeed = 0;
}
//call for position update after valid NMEA sentence
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 4;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#region Roll
if (mc.rollRaw != 9999)
{
//calculate how far the antenna moves based on sidehill roll
double roll = Math.Sin(glm.toRadians(mc.rollRaw/16.0));
rollCorrectionDistance = Math.Abs(roll * vehicle.antennaHeight);
// tilt to left is positive **** important!!
if (roll > 0)
{
pn.easting = (Math.Cos(fixHeading) * rollCorrectionDistance) + pn.easting;
pn.northing = (Math.Sin(fixHeading) * -rollCorrectionDistance) + pn.northing;
}
else
{
pn.easting = (Math.Cos(fixHeading) * -rollCorrectionDistance) + pn.easting;
pn.northing = (Math.Sin(fixHeading) * rollCorrectionDistance) + pn.northing;
}
}
//tiltDistance = (pitch * vehicle.antennaHeight);
////pn.easting = (Math.Sin(fixHeading) * tiltDistance) + pn.easting;
//pn.northing = (Math.Cos(fixHeading) * tiltDistance) + pn.northing;
#endregion Roll
#region Step Fix
//grab the most current fix and save the distance from the last fix
distanceCurrentStepFix = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
fixStepDist = distanceCurrentStepFix;
//if min distance isn't exceeded, keep adding old fixes till it does
if (distanceCurrentStepFix <= minFixStepDist)
{
for (currentStepFix = 0; currentStepFix < totalFixSteps; currentStepFix++)
{
fixStepDist += stepFixPts[currentStepFix].heading;
if (fixStepDist > minFixStepDist)
{
//if we reached end, keep the oldest and stay till distance is exceeded
if (currentStepFix < (totalFixSteps - 1)) currentStepFix++;
isFixHolding = false;
break;
}
else isFixHolding = true;
}
}
// only takes a single fix to exceeed min distance
else currentStepFix = 0;
//if total distance is less then the addition of all the fixes, keep last one as reference
if (isFixHolding)
{
if (isFixHoldLoaded == false)
{
vHold = stepFixPts[(totalFixSteps - 1)];
isFixHoldLoaded = true;
}
//cycle thru like normal
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
//fill in the latest distance and fix
stepFixPts[0].heading = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
stepFixPts[0].easting = pn.easting;
stepFixPts[0].northing = pn.northing;
//reload the last position that was triggered.
stepFixPts[(totalFixSteps - 1)].heading = pn.Distance(vHold.northing, vHold.easting, stepFixPts[(totalFixSteps - 1)].northing, stepFixPts[(totalFixSteps - 1)].easting);
stepFixPts[(totalFixSteps - 1)].easting = vHold.easting;
stepFixPts[(totalFixSteps - 1)].northing = vHold.northing;
}
else //distance is exceeded, time to do all calcs and next frame
{
//positions and headings
CalculatePositionHeading();
//get rid of hold position
isFixHoldLoaded = false;
//don't add the total distance again
stepFixPts[(totalFixSteps - 1)].heading = 0;
//grab sentences for logging
if (isLogNMEA)
{
if (ct.isContourOn)
{
pn.logNMEASentence.Append(recvSentenceSettings);
}
}
//To prevent drawing high numbers of triangles, determine and test before drawing vertex
sectionTriggerDistance = pn.Distance(pn.northing, pn.easting, prevSectionPos.northing, prevSectionPos.easting);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance)
AddSectionContourPathPoints();
//test if travelled far enough for new boundary point
double boundaryDistance = pn.Distance(pn.northing, pn.easting, prevBoundaryPos.northing, prevBoundaryPos.easting);
if (boundaryDistance > boundaryTriggerDistance) AddBoundaryAndPerimiterPoint();
//calc distance travelled since last GPS fix
distance = pn.Distance(pn.northing, pn.easting, prevFix.northing, prevFix.easting);
if ((userDistance += distance) > 3000) userDistance = 0; ;//userDistance can be reset
//most recent fixes are now the prev ones
prevFix.easting = pn.easting; prevFix.northing = pn.northing;
//load up history with valid data
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
stepFixPts[0].heading = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
stepFixPts[0].easting = pn.easting;
stepFixPts[0].northing = pn.northing;
}
#endregion fix
#region AutoSteer
guidanceLineDistanceOff = 32000; //preset the values
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
if (ABLine.isABLineSet && !ct.isContourBtnOn)
{
ABLine.GetCurrentABLine();
if (yt.isRecordingYouTurn)
{
//save reference of first point
if (yt.youFileList.Count == 0)
{
vec2 start = new vec2(pn.easting, pn.northing);
yt.youFileList.Add(start);
}
else
{
//keep adding points
vec2 point = new vec2(pn.easting - yt.youFileList[0].easting, pn.northing - yt.youFileList[0].northing);
yt.youFileList.Add(point);
}
}
}
// autosteer at full speed of updates
if (!isAutoSteerBtnOn) //32020 means auto steer is off
{
guidanceLineDistanceOff = 32020;
}
// If Drive button enabled be normal, or just fool the autosteer and fill values
if (!isInFreeDriveMode)
{
//fill up0 the auto steer array with new values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0);
mc.autoSteerData[mc.sdDistanceHi] = (byte)(guidanceLineDistanceOff >> 8);
mc.autoSteerData[mc.sdDistanceLo] = (byte)guidanceLineDistanceOff;
mc.autoSteerData[mc.sdSteerAngleHi] = (byte)(guidanceLineSteerAngle >> 8);
mc.autoSteerData[mc.sdSteerAngleLo] = (byte)guidanceLineSteerAngle;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerDataOutToPort();
SendUDPMessage(guidanceLineSteerAngle + "," + guidanceLineDistanceOff);
}
else
{
//fill up the auto steer array with free drive values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0 + 8);
//make steer module think everything is normal
mc.autoSteerData[mc.sdDistanceHi] = (byte)(0);
mc.autoSteerData[mc.sdDistanceLo] = (byte)0;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerDataOutToPort();
}
#endregion
//do the relayRateControl
if (rc.isRateControlOn)
{
rc.CalculateRateLitersPerMinute();
mc.relayRateData[mc.rdRateSetPointHi] = (byte)((Int16)(rc.rateSetPoint * 100.0) >> 8);
mc.relayRateData[mc.rdRateSetPointLo] = (byte)(rc.rateSetPoint * 100.0);
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm function BuildRelayByte()
}
else
{
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm fx BuildRelayByte()
}
//send out the port
RateRelayOutToPort(mc.relayRateData, AgOpenGPS.CModuleComm.numRelayRateDataItems);
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
//do the youturn logic every half second
if (boundary.isSet && (youTurnCounter++ > (fixUpdateHz>>2)))
{
//reset the counter
youTurnCounter = 0;
//are we in boundary? Then calc a distance
if (boundary.IsPrePointInPolygon(pivotAxlePos))
{
bool isVehicleInsideBoundary = boundary.IsPrePointInPolygon(pivotAxlePos);
if (isVehicleInsideBoundary)
{
boundary.FindClosestBoundaryPoint(pivotAxlePos);
if ((int)boundary.closestBoundaryPt.easting != -1)
{
distPt = pn.Distance(pivotAxlePos.northing, pivotAxlePos.easting, boundary.closestBoundaryPt.northing, boundary.closestBoundaryPt.easting);
}
else distPt = -2;
}
else distPt = -2;
}
else distPt = -2;
//trigger the "its ready to generate a youturn when 50m away" but don't make it just yet
if (distPt < 50.0 && distPt > 40 && !yt.isAutoTriggered && yt.isAutoYouTurnEnabled)
{
yt.isAutoTriggered = true;
//data buffer for pixels read from off screen buffer
byte[] grnPix = new byte[401];
//read a pixel line across full buffer width
OpenGL gl = openGLControlBack.OpenGL;
gl.ReadPixels(0, 205, 399, 1, OpenGL.GL_GREEN, OpenGL.GL_UNSIGNED_BYTE, grnPix);
//set up the positions to scan in the array for applied
int leftPos = vehicle.rpXPosition - 15;
if (leftPos < 0) leftPos = 0;
int rightPos = vehicle.rpXPosition + vehicle.rpWidth + 15;
if (rightPos > 399) rightPos = 399;
//do we need a left or right turn
bool isGrnOnLeft = false, isGrnOnRight = false;
//green on left means turn right
for (int j = leftPos; j < vehicle.rpXPosition; j++)
{ if (grnPix[j] > 50) isGrnOnLeft = true; else isGrnOnLeft = false; }
//green on right means turn left
for (int j = (rightPos - 10); j < rightPos; j++)
{ if (grnPix[j] > 50) isGrnOnRight = true; else isGrnOnRight = false; }
//one side or the other - but not both
if (!isGrnOnLeft && isGrnOnRight || isGrnOnLeft && !isGrnOnRight)
{
//set point and save to start measuring from
yt.isAutoPointSet = true;
yt.autoYouTurnTriggerPoint = pivotAxlePos;
if (isGrnOnRight) yt.isAutoTurnRight = false;
else yt.isAutoTurnRight = true;
}
//can't determine which way to turn, so pick opposite of last turn
else
{
yt.isAutoPointSet = true;
yt.autoYouTurnTriggerPoint = pivotAxlePos;
//just do the opposite of last turn
yt.isAutoTurnRight = !yt.isLastAutoTurnRight;
yt.isLastAutoTurnRight = !yt.isLastAutoTurnRight;
}
//modify the buttons to show the correct turn direction
if (yt.isAutoTurnRight)
{
AutoYouTurnButtonsRightTurn(); }
else
{
AutoYouTurnButtonsLeftTurn();
}
}
distanceToStartAutoTurn = -1;
//start counting down
if (yt.isAutoPointSet && yt.isAutoYouTurnEnabled)
{
//if we are too much off track, pointing wrong way, kill the turn
if ((Math.Abs(guidanceLineSteerAngle) > 50) && (Math.Abs(ABLine.distanceFromCurrentLine) > 500))
{
yt.CancelYouTurn();
distanceToStartAutoTurn = -1;
autoTurnInProgressBar = 0;
AutoYouTurnButtonsReset();
}
else
{
//how far have we gone since youturn request was triggered
distanceToStartAutoTurn = pn.Distance(pivotAxlePos.northing, pivotAxlePos.easting, yt.autoYouTurnTriggerPoint.northing, yt.autoYouTurnTriggerPoint.easting);
autoTurnInProgressBar = (int)(distanceToStartAutoTurn / (50 + yt.startYouTurnAt) * 100);
if (distanceToStartAutoTurn > (50 + yt.startYouTurnAt))
{
//keep from running this again since youturn is plotted now
yt.isAutoPointSet = false;
autoTurnInProgressBar = 0;
yt.isLastAutoTurnRight = yt.isAutoTurnRight;
yt.BuildYouTurnListToRight(yt.isAutoTurnRight);
}
}
}
}
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
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 BuildHeadland()
{
//count the points from the boundary
int ptCount = mf.boundz.ptList.Count;
//first find out which side is inside the boundary
heading = Math.Atan2(mf.boundz.ptList[3].easting - mf.boundz.ptList[4].easting
, mf.boundz.ptList[3].northing - mf.boundz.ptList[4].northing);
oneSide = glm.PIBy2;
point2.easting = mf.boundz.ptList[3].easting - (Math.Sin(oneSide + heading) * 2.0);
point2.northing = mf.boundz.ptList[3].northing - (Math.Cos(oneSide + heading) * 2.0);
if (!mf.boundz.IsPointInsideBoundary(point2))
{
oneSide *= -1.0;
}
//grab an array
vec4[] bnd = new vec4[ptCount];
vec2[] hlnd = new vec2[ptCount];
//clear the headland point list
mf.hl.ptList.Clear();
//determine how wide a headland space
double totalHeadWidth = mf.vehicle.toolWidth * (double)nudWidths.Value;
bnd[0].x = mf.boundz.ptList[0].easting;
bnd[0].y = mf.boundz.ptList[0].northing;
for (int i = 1; i < ptCount - 1; i++)
{
bnd[i].x = mf.boundz.ptList[i].easting;
bnd[i].y = mf.boundz.ptList[i].northing;
bnd[i - 1].k = Math.Atan2(bnd[i - 1].x - bnd[i].x, bnd[i - 1].y - bnd[i].y);
point.easting = bnd[i - 1].x - (Math.Sin(bnd[i - 1].k + oneSide) * totalHeadWidth);
point.northing = bnd[i - 1].y - (Math.Cos(bnd[i - 1].k + oneSide) * totalHeadWidth);
//only add if inside actual field boundary
if (mf.boundz.IsPointInsideBoundary(point))
{
mf.hl.ptList.Add(point);
}
}
int headCount = mf.hl.ptList.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 = mf.pn.Distance(mf.boundz.ptList[i], mf.hl.ptList[j]);
if (distance < (totalHeadWidth * 0.98))
{
mf.hl.ptList.RemoveAt(j);
headCount = mf.hl.ptList.Count;
j = 0;
}
}
}
//fix the gaps and overlaps
FixHeadlandList();
//pre calculate all the constants and multiples
mf.hl.PreCalcHeadlandLines();
//Enable the save ok button
btnOK.Enabled = true;
double area = mf.hl.CalculateHeadlandArea();
if (mf.isMetric)
{
lblArea.Text = Math.Round(area * 0.0001, 1) + " Ha";
}
else
{
lblArea.Text = Math.Round(area * 0.000247105, 1) + " Ac";
}
}
public void BuildTram()
{
mf.tram.BuildTramBnd();
mf.tram.tramList?.Clear();
mf.tram.tramArr?.Clear();
List <vec2> tramRef = new List <vec2>();
bool isBndExist = mf.bnd.bndList.Count != 0;
double pass = 0.5;
double hsin = Math.Sin(abHeading);
double hcos = Math.Cos(abHeading);
//divide up the AB line into segments
vec2 P1 = new vec2();
for (int i = 0; i < 3200; i += 4)
{
P1.easting = (hsin * i) + refABLineP1.easting;
P1.northing = (hcos * i) + refABLineP1.northing;
tramRef.Add(P1);
}
//create list of list of points of triangle strip of AB Highlight
double headingCalc = abHeading + glm.PIBy2;
hsin = Math.Sin(headingCalc);
hcos = Math.Cos(headingCalc);
mf.tram.tramList?.Clear();
mf.tram.tramArr?.Clear();
//no boundary starts on first pass
int cntr = 0;
if (isBndExist)
{
cntr = 1;
}
for (int i = cntr; i < mf.tram.passes; i++)
{
mf.tram.tramArr = new List <vec2>
{
Capacity = 128
};
mf.tram.tramList.Add(mf.tram.tramArr);
for (int j = 0; j < tramRef.Count; j++)
{
P1.easting = (hsin * ((mf.tram.tramWidth * (pass + i)) - mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + tramRef[j].easting;
P1.northing = (hcos * ((mf.tram.tramWidth * (pass + i)) - mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + tramRef[j].northing;
if (!isBndExist || mf.bnd.bndList[0].fenceLineEar.IsPointInPolygon(P1))
{
mf.tram.tramArr.Add(P1);
}
}
}
for (int i = cntr; i < mf.tram.passes; i++)
{
mf.tram.tramArr = new List <vec2>
{
Capacity = 128
};
mf.tram.tramList.Add(mf.tram.tramArr);
for (int j = 0; j < tramRef.Count; j++)
{
P1.easting = (hsin * ((mf.tram.tramWidth * (pass + i)) + mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + tramRef[j].easting;
P1.northing = (hcos * ((mf.tram.tramWidth * (pass + i)) + mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + tramRef[j].northing;
if (!isBndExist || mf.bnd.bndList[0].fenceLineEar.IsPointInPolygon(P1))
{
mf.tram.tramArr.Add(P1);
}
}
}
tramRef?.Clear();
//outside tram
if (mf.bnd.bndList.Count == 0 || mf.tram.passes != 0)
{
//return;
}
}
public static double Distance(vec2 first, double east, double north)
{
return(Math.Sqrt(
Math.Pow(first.easting - east, 2)
+ Math.Pow(first.northing - north, 2)));
}
public void GetCurrentABLine(vec3 pivot, vec3 steer)
{
double dx, dy;
if ((mf.secondsSinceStart - lastSecond) > 2)
{
lastSecond = mf.secondsSinceStart;
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.tool.toolWidth - mf.tool.toolOverlap;
//x2-x1
dx = refABLineP2.easting - refABLineP1.easting;
//z2-z1
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
isOnRightSideRefLine = distanceFromRefLine > 0;
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.tool.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) * 1200);
currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 1200);
currentABLineP2.easting = point1.easting + (Math.Sin(abHeading) * 1200);
currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 1200);
currentABLineP1.heading = abHeading;
currentABLineP2.heading = abHeading;
}
if (mf.isStanleyUsed)
{
bool isValid = true;
mf.gyd.StanleyGuidanceABLine(currentABLineP1, currentABLineP2, pivot, steer, isValid);
}
else
{
isABRefSameAsVehicleHeading = Math.PI - Math.Abs(Math.Abs(pivot.heading - abHeading) - Math.PI) < glm.PIBy2;
//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));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLinePivot > 0;
//integral slider is set to 0
if (mf.vehicle.purePursuitIntegralGain != 0)
{
pivotDistanceError = distanceFromCurrentLinePivot * 0.2 + pivotDistanceError * 0.8;
if (counter2++ > 4)
{
pivotDerivative = pivotDistanceError - pivotDistanceErrorLast;
pivotDistanceErrorLast = pivotDistanceError;
counter2 = 0;
pivotDerivative *= 2;
//limit the derivative
//if (pivotDerivative > 0.03) pivotDerivative = 0.03;
//if (pivotDerivative < -0.03) pivotDerivative = -0.03;
//if (Math.Abs(pivotDerivative) < 0.01) pivotDerivative = 0;
}
//pivotErrorTotal = pivotDistanceError + pivotDerivative;
if (mf.isAutoSteerBtnOn &&
Math.Abs(pivotDerivative) < (0.1) &&
mf.avgSpeed > 2.5 &&
!mf.yt.isYouTurnTriggered)
//&& 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;
}
//absolute the distance
distanceFromCurrentLinePivot = Math.Abs(distanceFromCurrentLinePivot);
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLinePivot);
//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;
if (isABSameAsVehicleHeading)
{
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 (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
distanceFromCurrentLinePivot = Math.Round(distanceFromCurrentLinePivot * 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)
{
distanceFromCurrentLinePivot *= -1.0;
}
}
//opposite way so right is left
else
{
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLinePivot *= -1.0;
}
isOnRightSideCurrentLine = !isOnRightSideCurrentLine;
}
mf.guidanceLineDistanceOff = mf.distanceDisplayPivot = (short)distanceFromCurrentLinePivot;
//mf.distanceDisplaySteer = (short)distanceFromCurrentLineSteer;
mf.distanceDisplaySteer = 0;// (short)distanceFromCurrentLineSteer;
mf.guidanceLineSteerAngle = (Int16)(steerAngleAB * 100);
}
if (mf.yt.isYouTurnTriggered)
{
//do the pure pursuit from youTurn
mf.yt.DistanceFromYouTurnLine();
}
}
//add the points for section, contour line points, Area Calc feature
private void AddSectionContourPathPoints()
{
//save the north & east as previous
prevSectionNorthing = toolNorthing;
prevSectionEasting = toolEasting;
//build the polygon to calculate area
if (periArea.isBtnPerimeterOn)
{
if (isAreaOnRight)
{
//Right side
vec2 point = new vec2(cosHeading * (section[vehicle.numberOfSections - 1].positionRight) + toolEasting,
sinHeading * (section[vehicle.numberOfSections - 1].positionRight) + toolNorthing);
periArea.periList.Add(point);
}
//draw on left side
else
{
//Right side
vec2 point = new vec2(cosHeading * (section[0].positionLeft) + toolEasting,
sinHeading * (section[0].positionLeft) + toolNorthing);
periArea.periList.Add(point);
}
}
if (isJobStarted)//add the pathpoint
{
// if non zero, at least one section is on.
int sectionCounter = 0;
//send the current and previous GPS fore/aft corrected fix to each section
for (int j = 0; j < vehicle.numberOfSections; j++)
{
if (section[j].isSectionOn)
{
section[j].AddPathPoint(toolNorthing, toolEasting, cosHeading, sinHeading);
sectionCounter++;
}
}
//Contour Base Track.... At least One section on, turn on if not
if (sectionCounter != 0)
{
//keep the line going, everything is on for recording path
if (ct.isContourOn) ct.AddPoint();
else { ct.StartContourLine(); ct.AddPoint(); }
}
//All sections OFF so if on, turn off
else { if (ct.isContourOn) { ct.StopContourLine(); } }
//Build contour line if close enough to a patch
ct.BuildContourGuidanceLine(pn.easting, pn.northing);
}
}
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;
}
}
//determine distance from youTurn guidance line
public void DistanceFromYouTurnLine()
{
//grab a copy from main
pivotAxlePosYT = mf.pivotAxlePos;
double minDistA = 1000000, minDistB = 1000000;
int ptCount = ytList.Count;
if (ptCount > 0)
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
double dist = ((pivotAxlePosYT.easting - ytList[t].x) * (pivotAxlePosYT.easting - ytList[t].x))
+ ((pivotAxlePosYT.northing - ytList[t].z) * (pivotAxlePosYT.northing - ytList[t].z));
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
double dx = ytList[B].x - ytList[A].x;
double dz = ytList[B].z - ytList[A].z;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//abHeading = Math.Atan2(dz, dx);
abHeading = ytList[A].y;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dz * mf.pn.fix.easting) - (dx * mf.pn.fix.northing) + (ytList[B].x
* ytList[A].z) - (ytList[B].z * ytList[A].x))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
//return and reset if too far away or end of the line
if (distanceFromCurrentLine > 5 || B >= ptCount - 3)
{
CompleteYouTurn();
return;
}
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivotAxlePosYT.easting - ytList[A].x) * (dx))
+ ((pivotAxlePosYT.northing - ytList[A].z) * (dz)))
/ ((dx * dx) + (dz * dz));
rEastYT = ytList[A].x + (U * (dx));
rNorthYT = ytList[A].z + (U * (dz));
//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 Whatever AB Line is meters look ahead
if (goalPointDistance < mf.vehicle.minLookAheadDistance)
{
goalPointDistance = mf.vehicle.minLookAheadDistance;
}
// used for calculating the length squared of next segment.
double tempDist = 0.0;
isABSameAsFixHeading = true;
distSoFar = mf.pn.Distance(ytList[B], rEastYT, rNorthYT);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointYT.easting = rEastYT + (Math.Sin(ytList[A].y) * goalPointDistance);
goalPointYT.northing = rNorthYT + (Math.Cos(ytList[A].y) * 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 = mf.pn.Distance(ytList[B], ytList[A]);
if ((tempDist + distSoFar) > goalPointDistance)
{
break; //will we go too far?
}
distSoFar += tempDist;
}
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointYT.easting = (((1 - t) * ytList[A].x) + (t * ytList[B].x));
goalPointYT.northing = (((1 - t) * ytList[A].z) + (t * ytList[B].z));
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceSquared = mf.pn.DistanceSquared(goalPointYT.northing, goalPointYT.easting, pivotAxlePosYT.northing, pivotAxlePosYT.easting);
//calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading;
ppRadiusYT = goalPointDistanceSquared / (2 * (((goalPointYT.easting - pivotAxlePosYT.easting) * Math.Cos(localHeading)) + ((goalPointYT.northing - pivotAxlePosYT.northing) * Math.Sin(localHeading))));
steerAngleYT = glm.toDegrees(Math.Atan(2 * (((goalPointYT.easting - pivotAxlePosYT.easting) * Math.Cos(localHeading))
+ ((goalPointYT.northing - pivotAxlePosYT.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));
if (steerAngleYT < -mf.vehicle.maxSteerAngle)
{
steerAngleYT = -mf.vehicle.maxSteerAngle;
}
if (steerAngleYT > mf.vehicle.maxSteerAngle)
{
steerAngleYT = mf.vehicle.maxSteerAngle;
}
if (ppRadiusYT < -500)
{
ppRadiusYT = -500;
}
if (ppRadiusYT > 500)
{
ppRadiusYT = 500;
}
radiusPointYT.easting = pivotAxlePosYT.easting + (ppRadiusYT * Math.Cos(localHeading));
radiusPointYT.northing = pivotAxlePosYT.northing + (ppRadiusYT * 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(steerAngleYT))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
{
steerAngleYT = glm.toDegrees(steerAngleYT > 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
if (isABSameAsFixHeading)
{
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
//opposite way so right is left
else
{
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleYT * 100);
}
else
{
CompleteYouTurn();
}
}
//determine distance from contour guidance line
public void DistanceFromContourLine()
{
//grab a copy from main
pivotAxlePosCT = mf.pivotAxlePos;
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 = ((pivotAxlePosCT.easting - ctList[t].x) * (pivotAxlePosCT.easting - ctList[t].x))
+ ((pivotAxlePosCT.northing - ctList[t].z) * (pivotAxlePosCT.northing - ctList[t].z));
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].x - ctList[A].x;
//z2-z1
double dz = ctList[B].z - ctList[A].z;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon) return;
//abHeading = Math.Atan2(dz, dx);
abHeading = ctList[A].y;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dz * pivotAxlePosCT.easting) - (dx * pivotAxlePosCT
.northing) + (ctList[B].x
* ctList[A].z) - (ctList[B].z * ctList[A].x))
/ 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 = (((pivotAxlePosCT.easting - ctList[A].x) * (dx))
+ ((pivotAxlePosCT.northing - ctList[A].z) * (dz)))
/ ((dx * dx) + (dz * dz));
rEastCT = ctList[A].x + (U * (dx));
rNorthCT = ctList[A].z + (U * (dz));
//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 = mf.pn.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].y) * goalPointDistance);
goalPointCT.northing = rNorthCT - (Math.Cos(ctList[A].y) * 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 = mf.pn.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].x) + (t * ctList[A].x));
goalPointCT.northing = (((1 - t) * ctList[B].z) + (t * ctList[A].z));
}
}
else
{
//counting up
isABSameAsFixHeading = true;
distSoFar = mf.pn.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].y) * goalPointDistance);
goalPointCT.northing = rNorthCT + (Math.Cos(ctList[A].y) * 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 = mf.pn.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].x) + (t * ctList[B].x));
goalPointCT.northing = (((1 - t) * ctList[A].z) + (t * ctList[B].z));
}
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceSquared = mf.pn.DistanceSquared(goalPointCT.northing, goalPointCT.easting, pivotAxlePosCT.northing, pivotAxlePosCT.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 - pivotAxlePosCT.easting) * Math.Cos(localHeading)) + ((goalPointCT.northing - pivotAxlePosCT.northing) * Math.Sin(localHeading))));
steerAngleCT = glm.toDegrees(Math.Atan(2 * (((goalPointCT.easting - pivotAxlePosCT.easting) * Math.Cos(localHeading))
+ ((goalPointCT.northing - pivotAxlePosCT.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 = pivotAxlePosCT.easting + (ppRadiusCT * Math.Cos(localHeading));
radiusPointCT.northing = pivotAxlePosCT.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)
{
//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 = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleCT*100);
//mf.guidanceLineHeadingDelta = (Int16)((Math.Atan2(Math.Sin(temp - mf.fixHeading),
// Math.Cos(temp - mf.fixHeading))) * 10000);
}
else
{
//invalid distance so tell AS module
distanceFromCurrentLine = 32000;
mf.guidanceLineDistanceOff = 32000;
}
}
//build the points and path of youturn to be scaled and transformed
public void BuildYouTurnListToRight(bool isTurnRight)
{
isYouTurnShapeDisplayed = true;
//point on AB line closest to pivot axle point from ABLine PurePursuit
rEastYT = mf.ABLine.rEastAB;
rNorthYT = mf.ABLine.rNorthAB;
isABSameAsFixHeading = mf.ABLine.isABSameAsFixHeading;
pivotAxlePosYT = mf.pivotAxlePos;
//grab the vehicle widths and offsets
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
double toolOffset = mf.vehicle.toolOffset * 2.0;
double turnOffset = 0;
abHeading = mf.ABLine.abHeading;
//turning right same as AB line
if ((isTurnRight && isABSameAsFixHeading) || (isTurnRight && !isABSameAsFixHeading))
{
turnOffset = (widthMinusOverlap + toolOffset);
}
//turning left same way as AB line
if ((!isTurnRight && isABSameAsFixHeading) || (!isTurnRight && !isABSameAsFixHeading))
{
turnOffset = (widthMinusOverlap - toolOffset);
}
numShapePoints = youFileList.Count;
vec4[] pt = new vec4[numShapePoints];
//Now put the shape into an array since lists are immutable
for (int i = 0; i < numShapePoints; i++)
{
pt[i].x = youFileList[i].easting;
pt[i].z = youFileList[i].northing;
}
//for (int i = 0; i < pt.Length; i++)
//{
// bool started = false;
// if (pt[i].x >= 5.0)
// {
// if (!started) started = true;
// //create a line of 10 points
// pt[i].x += ((rowSkipsWidth - 1) * 10);
// }
//}
//start of path on the origin. Mirror the shape if left turn
if (!isTurnRight)
{
for (int i = 0; i < pt.Length; i++)
{
pt[i].x *= -1;
}
}
//scaling - Drawing is 10m wide so find ratio of tool width
double scale = turnOffset * 0.1;
for (int i = 0; i < pt.Length; i++)
{
pt[i].x *= scale * rowSkipsWidth;
pt[i].z *= scale * rowSkipsHeight;
}
//rotate pattern to match AB Line heading
for (int i = 0; i < pt.Length; i++)
{
double xr, yr;
if (isABSameAsFixHeading)
{
xr = (Math.Cos(-abHeading) * pt[i].x) - (Math.Sin(-abHeading) * pt[i].z);
yr = (Math.Sin(-abHeading) * pt[i].x) + (Math.Cos(-abHeading) * pt[i].z);
}
else
{
xr = (Math.Cos(-abHeading + Math.PI) * pt[i].x) - (Math.Sin(-abHeading + Math.PI) * pt[i].z);
yr = (Math.Sin(-abHeading + Math.PI) * pt[i].x) + (Math.Cos(-abHeading + Math.PI) * pt[i].z);
}
pt[i].x = xr + rEastYT;
pt[i].z = yr + rNorthYT;
pt[i].k = 0;
pt[i].y = Math.Atan2(pt[i].z, pt[i].x);
ytList.Add(pt[i]);
}
}
//called when the 45 m mark is reached before headland
public void YouTurnTrigger()
{
//trigger pulled and make box double ended
isYouTurnTriggered = true;
isSequenceTriggered = true;
//our direction heading into turn
isABLineSameAsHeadingAtTrigger = mf.ABLine.isABSameAsFixHeading;
//data buffer for pixels read from off screen buffer
byte[] grnPix = new byte[401];
//read a pixel line across full buffer width
glb.ReadPixels(0, 205, 399, 1, OpenGL.GL_GREEN, OpenGL.GL_UNSIGNED_BYTE, grnPix);
//set up the positions to scan in the array for applied
int leftPos = mf.vehicle.rpXPosition - 15;
if (leftPos < 0)
{
leftPos = 0;
}
int rightPos = mf.vehicle.rpXPosition + mf.vehicle.rpWidth + 15;
if (rightPos > 399)
{
rightPos = 399;
}
//do we need a left or right turn
bool isGrnOnLeft = false, isGrnOnRight = false;
//green on left means turn right
for (int j = leftPos; j < mf.vehicle.rpXPosition; j++)
{
isGrnOnLeft = grnPix[j] > 50;
}
//green on right means turn left
for (int j = (rightPos - 10); j < rightPos; j++)
{
isGrnOnRight = grnPix[j] > 50;
}
//set point and save to start measuring from
isYouTurnTriggerPointSet = true;
youTurnTriggerPoint = mf.pivotAxlePos;
//one side or the other - but not both Exclusive Or
if (isGrnOnLeft ^ isGrnOnRight)
{
isYouTurnRight = !isGrnOnRight;
}
else //can't determine which way to turn, so pick opposite of last turn
{
//just do the opposite of last turn
isYouTurnRight = !isLastYouTurnRight;
isLastYouTurnRight = !isLastYouTurnRight;
}
//modify the buttons to show the correct turn direction
if (isYouTurnRight)
{
mf.AutoYouTurnButtonsRightTurn();
}
else
{
mf.AutoYouTurnButtonsLeftTurn();
}
}
private void oglSelf_MouseDown(object sender, MouseEventArgs e)
{
Point pt = oglSelf.PointToClient(Cursor.Position);
//Convert to Origin in the center of window, 800 pixels
fixPt.X = pt.X - 400;
fixPt.Y = (800 - pt.Y - 400);
vec3 plotPt = new vec3
{
//convert screen coordinates to field coordinates
easting = ((double)fixPt.X) * (double)maxFieldDistance / 723.0 * gain,
northing = ((double)fixPt.Y) * (double)maxFieldDistance / 723.0 * gain,
heading = 0
};
plotPt.easting += fieldCenterX;
plotPt.northing += fieldCenterY;
lblX.Text = plotPt.easting.ToString();
lblY.Text = plotPt.northing.ToString();
mf.self.pint.easting = plotPt.easting;
mf.self.pint.northing = plotPt.northing;
if (isA)
{
mf.self.linePtList?.Clear();
mf.self.lineList?.Clear();
mf.ABLine.ResetABLine();
lastABLineP1.easting = 9999; lastABLineP1.northing = 9999;
lastABLineP2.easting = 9999; lastABLineP2.northing = 9999;
mf.ABLine.refPoint1.easting = plotPt.easting;
mf.ABLine.refPoint1.northing = plotPt.northing;
isA = false;
isB = true;
}
else if (isB)
{
mf.ABLine.refPoint2.easting = plotPt.easting;
mf.ABLine.refPoint2.northing = plotPt.northing;
isA = false;
isB = false;
isC = true;
//calculate the AB Heading
mf.ABLine.abHeading = Math.Atan2(mf.ABLine.refPoint2.easting - mf.ABLine.refPoint1.easting, mf.ABLine.refPoint2.northing - mf.ABLine.refPoint1.northing);
if (mf.ABLine.abHeading < 0)
{
mf.ABLine.abHeading += glm.twoPI;
}
//sin x cos y for endpoints, opposite for additional lines
mf.ABLine.refABLineP1.easting = mf.ABLine.refPoint1.easting - (Math.Sin(mf.ABLine.abHeading) * 1500.0);
mf.ABLine.refABLineP1.northing = mf.ABLine.refPoint1.northing - (Math.Cos(mf.ABLine.abHeading) * 1500.0);
mf.ABLine.refABLineP2.easting = mf.ABLine.refPoint1.easting + (Math.Sin(mf.ABLine.abHeading) * 1500.0);
mf.ABLine.refABLineP2.northing = mf.ABLine.refPoint1.northing + (Math.Cos(mf.ABLine.abHeading) * 1500.0);
mf.ABLine.isABLineSet = true;
mf.FileSaveABLine();
}
//borrowed snap code for last line of field
else if (isC)
{
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
//x2-x1
double dx = mf.ABLine.refABLineP2.easting - mf.ABLine.refABLineP1.easting;
//z2-z1
double dy = mf.ABLine.refABLineP2.northing - mf.ABLine.refABLineP1.northing;
//how far are we away from the reference line at 90 degrees
double distanceFromRefLine = ((dy * plotPt.easting) - (dx * plotPt.northing)
+ (mf.ABLine.refABLineP2.easting * mf.ABLine.refABLineP1.northing)
- (mf.ABLine.refABLineP2.northing * mf.ABLine.refABLineP1.easting))
/ Math.Sqrt((dy * dy) + (dx * dx));
//sign of distance determines which side of line we are on
int refLineSide;
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
double howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
//generate that pass number as signed integer
mf.self.lastPassNumber = Convert.ToInt32(refLineSide * howManyPathsAway);
//calculate the new point that is number of implement widths over
double toolOffset = mf.vehicle.toolOffset;
vec2 point1 = new vec2((Math.Cos(-mf.ABLine.abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide)
- toolOffset)) + mf.ABLine.refPoint1.easting,
(Math.Sin(-mf.ABLine.abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide)
- toolOffset)) + mf.ABLine.refPoint1.northing);
//create the new line extent points for current ABLine based on original heading of AB line
lastABLineP1.easting = point1.easting - (Math.Sin(mf.ABLine.abHeading) * 1500.0);
lastABLineP1.northing = point1.northing - (Math.Cos(mf.ABLine.abHeading) * 1500.0);
lastABLineP2.easting = point1.easting + (Math.Sin(mf.ABLine.abHeading) * 1500.0);
lastABLineP2.northing = point1.northing + (Math.Cos(mf.ABLine.abHeading) * 1500.0);
isC = false;
isA = true;
lblPasses.Text = mf.self.lastPassNumber.ToString();
//build the lines, determine bounds
mf.self.BuildLines();
int cnt = mf.self.cellDecList.Count;
tbox1.Text = "";
for (int i = 0; i < cnt; i++)
{
tbox1.Text += i + "\t" + mf.self.cellDecList[i].lines + "\t" + mf.self.cellDecList[i].cells + "\r\n";
}
}
}
//call for position update after valid NMEA sentence
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 4;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#region Roll
rollUsed = 0;
if ((ahrs.isRollBrick | ahrs.isRollDogs | ahrs.isRollPAOGI) && mc.rollRaw != 9999)
{
//for charting in GPS Data window
eastingBeforeRoll = pn.fix.easting;
rollUsed = (double)mc.rollRaw/16;
//calculate how far the antenna moves based on sidehill roll
double roll = Math.Sin(glm.toRadians((mc.rollRaw - ahrs.rollZero) * 0.0625));
rollCorrectionDistance = Math.Abs(roll * vehicle.antennaHeight);
// roll to left is positive **** important!!
if (roll > 0)
{
pn.fix.easting = (Math.Cos(fixHeading) * rollCorrectionDistance) + pn.fix.easting;
pn.fix.northing = (Math.Sin(fixHeading) * -rollCorrectionDistance) + pn.fix.northing;
}
else
{
pn.fix.easting = (Math.Cos(fixHeading) * -rollCorrectionDistance) + pn.fix.easting;
pn.fix.northing = (Math.Sin(fixHeading) * rollCorrectionDistance) + pn.fix.northing;
}
//for charting the position after roll adjustment
eastingAfterRoll = pn.fix.easting;
}
else
{
eastingAfterRoll = pn.fix.easting;
eastingBeforeRoll = pn.fix.easting;
}
//pitchDistance = (pitch * vehicle.antennaHeight);
////pn.fix.easting = (Math.Sin(fixHeading) * pitchDistance) + pn.fix.easting;
//pn.fix.northing = (Math.Cos(fixHeading) * pitchDistance) + pn.fix.northing;
#endregion Roll
#region Step Fix
//**** heading of the vec3 structure is used for distance in Step fix!!!!!
//grab the most current fix and save the distance from the last fix
distanceCurrentStepFix = glm.Distance(pn.fix, stepFixPts[0]);
fixStepDist = distanceCurrentStepFix;
//if min distance isn't exceeded, keep adding old fixes till it does
if (distanceCurrentStepFix <= minFixStepDist)
{
for (currentStepFix = 0; currentStepFix < totalFixSteps; currentStepFix++)
{
fixStepDist += stepFixPts[currentStepFix].heading;
if (fixStepDist > minFixStepDist)
{
//if we reached end, keep the oldest and stay till distance is exceeded
if (currentStepFix < (totalFixSteps - 1)) currentStepFix++;
isFixHolding = false;
break;
}
else isFixHolding = true;
}
}
// only takes a single fix to exceeed min distance
else currentStepFix = 0;
//if total distance is less then the addition of all the fixes, keep last one as reference
if (isFixHolding)
{
if (isFixHoldLoaded == false)
{
vHold = stepFixPts[(totalFixSteps - 1)];
isFixHoldLoaded = true;
}
//cycle thru like normal
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
//fill in the latest distance and fix
stepFixPts[0].heading = glm.Distance(pn.fix, stepFixPts[0]);
stepFixPts[0].easting = pn.fix.easting;
stepFixPts[0].northing = pn.fix.northing;
//reload the last position that was triggered.
stepFixPts[(totalFixSteps - 1)].heading = glm.Distance(vHold, stepFixPts[(totalFixSteps - 1)]);
stepFixPts[(totalFixSteps - 1)].easting = vHold.easting;
stepFixPts[(totalFixSteps - 1)].northing = vHold.northing;
}
else //distance is exceeded, time to do all calcs and next frame
{
//positions and headings
CalculatePositionHeading();
//get rid of hold position
isFixHoldLoaded = false;
//don't add the total distance again
stepFixPts[(totalFixSteps - 1)].heading = 0;
//grab sentences for logging
if (isLogNMEA) { if (ct.isContourOn) { pn.logNMEASentence.Append(recvSentenceSettings); } }
//To prevent drawing high numbers of triangles, determine and test before drawing vertex
sectionTriggerDistance = glm.Distance(pn.fix, prevSectionPos);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance && isJobStarted)
{
AddSectionContourPathPoints();
}
//test if travelled far enough for new boundary point
double boundaryDistance = glm.Distance(pn.fix, prevBoundaryPos);
if (boundaryDistance > boundaryTriggerDistance) AddBoundaryAndPerimiterPoint();
//calc distance travelled since last GPS fix
distance = glm.Distance(pn.fix, prevFix);
if ((userDistance += distance) > 3000) userDistance = 0; ;//userDistance can be reset
//most recent fixes are now the prev ones
prevFix.easting = pn.fix.easting; prevFix.northing = pn.fix.northing;
//load up history with valid data
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
stepFixPts[0].heading = glm.Distance(pn.fix, stepFixPts[0]);
stepFixPts[0].easting = pn.fix.easting;
stepFixPts[0].northing = pn.fix.northing;
}
#endregion fix
#region AutoSteer
guidanceLineDistanceOff = 32000; //preset the values
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
if (ABLine.isABLineSet && !ct.isContourBtnOn)
{
ABLine.GetCurrentABLine();
if (yt.isRecordingCustomYouTurn)
{
//save reference of first point
if (yt.youFileList.Count == 0)
{
vec2 start = new vec2(pn.fix.easting, pn.fix.northing);
yt.youFileList.Add(start);
}
else
{
//keep adding points
vec2 point = new vec2(pn.fix.easting - yt.youFileList[0].easting, pn.fix.northing - yt.youFileList[0].northing);
yt.youFileList.Add(point);
}
}
}
// autosteer at full speed of updates
if (!isAutoSteerBtnOn) //32020 means auto steer is off
{
guidanceLineDistanceOff = 32020;
}
// If Drive button enabled be normal, or just fool the autosteer and fill values
if (!ast.isInFreeDriveMode)
{
//fill up0 the auto steer array with new values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0);
mc.autoSteerData[mc.sdDistanceHi] = (byte)(guidanceLineDistanceOff >> 8);
mc.autoSteerData[mc.sdDistanceLo] = (byte)guidanceLineDistanceOff;
mc.autoSteerData[mc.sdSteerAngleHi] = (byte)(guidanceLineSteerAngle >> 8);
mc.autoSteerData[mc.sdSteerAngleLo] = (byte)guidanceLineSteerAngle;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerDataOutToPort();
//autosteer data out the UDP Steer port
SendUDPMessage(guidanceLineSteerAngle + "," + guidanceLineDistanceOff);
}
else
{
//fill up the auto steer array with free drive values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0 + 8);
//make steer module think everything is normal
mc.autoSteerData[mc.sdDistanceHi] = (byte)(0);
mc.autoSteerData[mc.sdDistanceLo] = (byte)0;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerDataOutToPort();
}
#endregion
#region relayRatecontrol
//do the relayRateControl
if (rc.isRateControlOn)
{
rc.CalculateRateLitersPerMinute();
mc.relayRateData[mc.rdRateSetPointHi] = (byte)((Int16)(rc.rateSetPoint * 100.0) >> 8);
mc.relayRateData[mc.rdRateSetPointLo] = (byte)(rc.rateSetPoint * 100.0);
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm function BuildRelayByte()
//youturn control byte is built in SerialComm BuildYouTurnByte()
}
else
{
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm.cs - function BuildRelayByte()
//youturn control byte is built in SerialComm BuildYouTurnByte()
}
//send out the port
RateRelayOutToPort(mc.relayRateData, AgOpenGPS.CModuleComm.numRelayRateDataItems);
#endregion
#region Youturn
//do the auto youturn logic if everything is on.
if (hl.isSet && yt.isYouTurnBtnOn && isAutoSteerBtnOn)
{
//figure out where we are
yt.isInBoundz = boundz.IsPointInsideBoundary(toolPos);
yt.isInWorkArea = hl.IsPointInsideHeadland(toolPos);
//Are we in the headland?
if (!yt.isInWorkArea && yt.isInBoundz) yt.isInHeadland = true;
else yt.isInHeadland = false;
//are we in boundary? Then calc a distance
if (yt.isInBoundz)
{
hl.FindClosestHeadlandPoint(pivotAxlePos);
if ((int)hl.closestHeadlandPt.easting != -1)
{
distPivot = glm.Distance(pivotAxlePos, hl.closestHeadlandPt);
}
else distPivot = -2;
}
else distPivot = -2;
//trigger the "its ready to generate a youturn when 25m away" but don't make it just yet
if (distPivot < 25.0 && distPivot > 22 && !yt.isYouTurnTriggered && yt.isInWorkArea)
{
//begin the whole process, all conditions are met
yt.YouTurnTrigger();
}
//Do the sequencing of functions around the turn.
if (yt.isSequenceTriggered)
{
yt.DoSequenceEvent();
}
distanceToStartAutoTurn = -1;
//start counting down - this is not run if shape is drawn
if (yt.isYouTurnTriggerPointSet && yt.isYouTurnBtnOn)
{
//if we are too much off track - 5 degrees 500 mm, pointing wrong way, kill the turn
if ((Math.Abs(guidanceLineSteerAngle) > 500) && (Math.Abs(ABLine.distanceFromCurrentLine) > 500))
{
yt.ResetYouTurnAndSequenceEvents();
}
else
{
//how far have we gone since youturn request was triggered
distanceToStartAutoTurn = glm.Distance(pivotAxlePos, yt.youTurnTriggerPoint);
//youTurnProgressBar = (int)(distanceToStartAutoTurn / (45 + yt.youTurnStartOffset) * 100);
if (distanceToStartAutoTurn > (25 + yt.youTurnStartOffset))
{
//keep from running this again since youturn is plotted now
yt.isYouTurnTriggerPointSet = false;
youTurnProgressBar = 0;
yt.isLastYouTurnRight = yt.isYouTurnRight;
yt.BuildYouTurnListToRight(yt.isYouTurnRight);
}
}
}
}
else //make sure youturn and sequence is off - we are not in normal turn here
{
if(yt.isYouTurnTriggered | yt.isSequenceTriggered)
{
yt.ResetYouTurnAndSequenceEvents();
}
}
#endregion
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
public void BuildTram()
{
mf.tram.BuildTramBnd();
tramList?.Clear();
tramArr?.Clear();
List <vec2> tramRef = new List <vec2>();
bool isBndExist = mf.bnd.bndArr.Count != 0;
double pass = 0.5;
double hsin = Math.Sin(abHeading);
double hcos = Math.Cos(abHeading);
//divide up the AB line into segments
vec2 P1 = new vec2();
for (int i = 0; i < 3200; i += 4)
{
P1.easting = (hsin * i) + refABLineP1.easting;
P1.northing = (hcos * i) + refABLineP1.northing;
tramRef.Add(P1);
}
//create list of list of points of triangle strip of AB Highlight
double headingCalc = abHeading + glm.PIBy2;
hsin = Math.Sin(headingCalc);
hcos = Math.Cos(headingCalc);
tramList?.Clear();
tramArr?.Clear();
for (int i = 0; i < mf.tram.passes; i++)
{
tramArr = new List <vec2>();
tramList.Add(tramArr);
for (int j = 0; j < tramRef.Count; j++)
{
P1.easting = (hsin * ((mf.tram.tramWidth * (pass + i)) - mf.tram.halfWheelTrack + mf.tram.abOffset)) + tramRef[j].easting;
P1.northing = (hcos * ((mf.tram.tramWidth * (pass + i)) - mf.tram.halfWheelTrack + mf.tram.abOffset)) + tramRef[j].northing;
if (isBndExist)
{
if (mf.bnd.bndArr[0].IsPointInsideBoundaryEar(P1))
{
tramArr.Add(P1);
P1.easting = (hsin * mf.tram.wheelTrack) + P1.easting;
P1.northing = (hcos * mf.tram.wheelTrack) + P1.northing;
tramArr.Add(P1);
}
}
else
{
tramArr.Add(P1);
P1.easting = (hsin * mf.tram.wheelTrack) + P1.easting;
P1.northing = (hcos * mf.tram.wheelTrack) + P1.northing;
tramArr.Add(P1);
}
}
}
tramRef?.Clear();
//outside tram
if (mf.bnd.bndArr.Count == 0 || mf.tram.passes != 0)
{
//return;
}
}
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)
{
if (refList == null || refList.Count < 5)
{
return;
}
//build new current ref line if required
if (!isCurveValid || ((mf.secondsSinceStart - lastSecond) > 0.66 &&
(!mf.isAutoSteerBtnOn || mf.mc.steerSwitchValue != 0)))
{
BuildCurveCurrentList(pivot);
}
double dist, dx, dz;
double minDistA = 1000000, minDistB = 1000000;
int ptCount = curList.Count;
if (ptCount > 0)
{
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
steerAngleCu = mf.yt.steerAngleYT;
distanceFromCurrentLinePivot = mf.yt.distanceFromCurrentLine;
goalPointCu = mf.yt.goalPointYT;
radiusPointCu.easting = mf.yt.radiusPointYT.easting;
radiusPointCu.northing = mf.yt.radiusPointYT.northing;
ppRadiusCu = mf.yt.ppRadiusYT;
}
else if (mf.isStanleyUsed)//Stanley
{
mf.gyd.StanleyGuidanceCurve(pivot, steer, ref curList);
}
else// Pure Pursuit ------------------------------------------
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
dist = glm.DistanceSquared(pivot, curList[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)
{
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
distanceFromCurrentLinePivot = ((dz * pivot.easting) - (dx * pivot.northing) + (curList[B].easting
* curList[A].northing) - (curList[B].northing * curList[A].easting))
/ Math.Sqrt((dz * dz) + (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 && mf.avgSpeed > 2.5 && Math.Abs(pivotDerivative) < 0.1)
{
//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;
}
// ** 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);
manualUturnHeading = curList[A].heading;
//double minx, maxx, miny, maxy;
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();
bool ReverseHeading = mf.isReverse ? !isHeadingSameWay : isHeadingSameWay;
int count = ReverseHeading ? 1 : -1;
vec3 start = new vec3(rEastCu, rNorthCu, 0);
double distSoFar = 0;
for (int i = ReverseHeading ? B : A; i < ptCount && i >= 0; i += count)
{
// used for calculating the length squared of next segment.
double tempDist = glm.Distance(start, curList[i]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
double j = (goalPointDistance - distSoFar) / tempDist; // the remainder to yet travel
goalPointCu.easting = (((1 - j) * start.easting) + (j * curList[i].easting));
goalPointCu.northing = (((1 - j) * start.northing) + (j * curList[i].northing));
break;
}
else
{
distSoFar += tempDist;
}
start = curList[i];
}
//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;
double localHeading;
if (ReverseHeading)
{
localHeading = glm.twoPI - mf.fixHeading + inty;
}
else
{
localHeading = glm.twoPI - mf.fixHeading - inty;
}
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 (mf.ahrs.imuRoll != 88888)
{
steerAngleCu += mf.ahrs.imuRoll * -mf.gyd.sideHillCompFactor;
}
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.avgSpeed * 0.277777)))
: (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.avgSpeed * 0.277777))));
}
if (!isHeadingSameWay)
{
distanceFromCurrentLinePivot *= -1.0;
}
//Convert to centimeters
mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero);
mf.guidanceLineSteerAngle = (short)(steerAngleCu * 100);
}
}
else
{
//invalid distance so tell AS module
distanceFromCurrentLinePivot = 32000;
mf.guidanceLineDistanceOff = 32000;
}
}
public static double DistanceSquared(vec2 first, vec2 second)
{
return(
Math.Pow(first.easting - second.easting, 2)
+ Math.Pow(first.northing - second.northing, 2));
}
public void BuildTram()
{
mf.tram.BuildTramBnd();
mf.tram.tramList?.Clear();
mf.tram.tramArr?.Clear();
bool isBndExist = mf.bnd.bndList.Count != 0;
double pass = 0.5;
int refCount = refList.Count;
int cntr = 0;
if (isBndExist)
{
cntr = 1;
}
for (int i = cntr; i <= mf.tram.passes; i++)
{
double distSqAway = (mf.tram.tramWidth * (i + 0.5) - mf.tram.halfWheelTrack + mf.tool.halfToolWidth)
* (mf.tram.tramWidth * (i + 0.5) - mf.tram.halfWheelTrack + mf.tool.halfToolWidth) * 0.999999;
mf.tram.tramArr = new List <vec2>
{
Capacity = 128
};
mf.tram.tramList.Add(mf.tram.tramArr);
for (int j = 0; j < refCount; j += 1)
{
vec2 point = new vec2(
(Math.Sin(glm.PIBy2 + refList[j].heading) *
((mf.tram.tramWidth * (pass + i)) - mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + refList[j].easting,
(Math.Cos(glm.PIBy2 + refList[j].heading) *
((mf.tram.tramWidth * (pass + i)) - mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + refList[j].northing);
bool Add = true;
for (int t = 0; t < refCount; t++)
{
//distance check to be not too close to ref line
double dist = ((point.easting - refList[t].easting) * (point.easting - refList[t].easting))
+ ((point.northing - refList[t].northing) * (point.northing - refList[t].northing));
if (dist < distSqAway)
{
Add = false;
break;
}
}
if (Add)
{
//a new point only every 2 meters
double dist = mf.tram.tramArr.Count > 0 ? ((point.easting - mf.tram.tramArr[mf.tram.tramArr.Count - 1].easting) * (point.easting - mf.tram.tramArr[mf.tram.tramArr.Count - 1].easting))
+ ((point.northing - mf.tram.tramArr[mf.tram.tramArr.Count - 1].northing) * (point.northing - mf.tram.tramArr[mf.tram.tramArr.Count - 1].northing)) : 3.0;
if (dist > 2)
{
//if inside the boundary, add
if (!isBndExist || mf.bnd.bndList[0].fenceLineEar.IsPointInPolygon(point))
{
mf.tram.tramArr.Add(point);
}
}
}
}
}
for (int i = cntr; i <= mf.tram.passes; i++)
{
double distSqAway = (mf.tram.tramWidth * (i + 0.5) + mf.tram.halfWheelTrack + mf.tool.halfToolWidth)
* (mf.tram.tramWidth * (i + 0.5) + mf.tram.halfWheelTrack + mf.tool.halfToolWidth) * 0.999999;
mf.tram.tramArr = new List <vec2>
{
Capacity = 128
};
mf.tram.tramList.Add(mf.tram.tramArr);
for (int j = 0; j < refCount; j += 1)
{
vec2 point = new vec2(
(Math.Sin(glm.PIBy2 + refList[j].heading) *
((mf.tram.tramWidth * (pass + i)) + mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + refList[j].easting,
(Math.Cos(glm.PIBy2 + refList[j].heading) *
((mf.tram.tramWidth * (pass + i)) + mf.tram.halfWheelTrack + mf.tool.halfToolWidth)) + refList[j].northing);
bool Add = true;
for (int t = 0; t < refCount; t++)
{
//distance check to be not too close to ref line
double dist = ((point.easting - refList[t].easting) * (point.easting - refList[t].easting))
+ ((point.northing - refList[t].northing) * (point.northing - refList[t].northing));
if (dist < distSqAway)
{
Add = false;
break;
}
}
if (Add)
{
//a new point only every 2 meters
double dist = mf.tram.tramArr.Count > 0 ? ((point.easting - mf.tram.tramArr[mf.tram.tramArr.Count - 1].easting) * (point.easting - mf.tram.tramArr[mf.tram.tramArr.Count - 1].easting))
+ ((point.northing - mf.tram.tramArr[mf.tram.tramArr.Count - 1].northing) * (point.northing - mf.tram.tramArr[mf.tram.tramArr.Count - 1].northing)) : 3.0;
if (dist > 2)
{
//if inside the boundary, add
if (!isBndExist || mf.bnd.bndList[0].fenceLineEar.IsPointInPolygon(point))
{
mf.tram.tramArr.Add(point);
}
}
}
}
}
}
}//end of open file
//function to open a previously saved field, Contour, Flags
public void FileOpenField(string _openType)
{
string fileAndDirectory;
//get the directory where the fields are stored
//string directoryName = System.IO.Path.GetDirectoryName(System.Reflection.Assembly.GetExecutingAssembly().Location)+ "\\fields\\";
if (_openType == "Resume")
{
//Either exit or update running save
fileAndDirectory = workingDirectory + currentFieldDirectory + "\\Field.fld";
if (!File.Exists(fileAndDirectory))
{
return;
}
}
//open file dialog instead
else
{
//create the dialog instance
OpenFileDialog ofd = new OpenFileDialog();
//the initial directory, fields, for the open dialog
ofd.InitialDirectory = workingDirectory;
//When leaving dialog put windows back where it was
ofd.RestoreDirectory = true;
//set the filter to text files only
ofd.Filter = "fld files (*.fld)|*.fld";
//was a file selected
if (ofd.ShowDialog() == DialogResult.Cancel)
{
return;
}
else
{
fileAndDirectory = ofd.FileName;
}
}
//Friday, February 10, 2017 --> 2:58:24 PM
//$FieldDir
//test_2017Feb10_02.55.46.PM
//$Offsets
//533210,5927965,12
//$Heading
//False
//0,0.2,0.2,0.3,0.3,0,0
//$Sections
//3
//4
//76
//-10.791,-10.964
//close the existing job and reset everything
this.JobClose();
//and open a new job
this.JobNew();
using (BinaryReader br = new BinaryReader(new FileStream(fileAndDirectory, FileMode.Open)))
{
string s = br.ReadString(); //Date/time
s = br.ReadString(); //$FieldDir
if (s.IndexOf("$FieldDir") == -1)
{
MessageBox.Show("Sections header is Corrupt"); JobClose(); return;
}
currentFieldDirectory = br.ReadString(); //read current directory
currentFieldDirectory = currentFieldDirectory.Trim();
//Offset header
s = br.ReadString(); //$Offsets
//read the Offsets
pn.utmEast = br.ReadInt32();
pn.utmNorth = br.ReadInt32();
pn.zone = br.ReadDouble();
worldGrid.CreateWorldGrid(pn.actualNorthing - pn.utmNorth, pn.actualEasting - pn.utmEast);
//AB Line header
s = br.ReadString(); //$Heading
//read the boolean if AB is set
bool b = br.ReadBoolean();
//If is true there is AB Line data
if (b)
{
//set gui image button on
btnABLine.Image = global::AgOpenGPS.Properties.Resources.ABLineOn;
//Heading, refPoint1x,z,refPoint2x,z
ABLine.abHeading = br.ReadDouble();
ABLine.refPoint1.x = br.ReadDouble();
ABLine.refPoint1.z = br.ReadDouble();
ABLine.refPoint2.x = br.ReadDouble();
ABLine.refPoint2.z = br.ReadDouble();
ABLine.tramPassEvery = br.ReadInt32();
ABLine.passBasedOn = br.ReadInt32();
ABLine.refABLineP1.x = ABLine.refPoint1.x - Math.Sin(ABLine.abHeading) * 10000.0;
ABLine.refABLineP1.z = ABLine.refPoint1.z - Math.Cos(ABLine.abHeading) * 10000.0;
ABLine.refABLineP2.x = ABLine.refPoint1.x + Math.Sin(ABLine.abHeading) * 10000.0;
ABLine.refABLineP2.z = ABLine.refPoint1.z + Math.Cos(ABLine.abHeading) * 10000.0;
ABLine.isABLineSet = true;
}
//false so just read and skip the heading line, reset btn image
else
{
btnABLine.Image = global::AgOpenGPS.Properties.Resources.ABLineOff;
br.ReadDouble();
br.ReadDouble();
br.ReadDouble();
br.ReadDouble();
br.ReadDouble();
br.ReadInt32();
br.ReadInt32();
}
//read the line $Sections
s = br.ReadString(); //$Sections
if (s.IndexOf("$Sections") == -1)
{
MessageBox.Show("Sections header is Corrupt"); JobClose(); return;
}
//read number of sections
int numSects = br.ReadInt32() - 1; //$Sections
//make sure sections in file matches sections set in current vehicle
if (vehicle.numOfSections != numSects)
{
MessageBox.Show("# of Sections doesn't match this field"); JobClose(); return;
}
//finally start loading triangles
vec2 vecFix = new vec2(0, 0);
for (int j = 0; j < vehicle.numOfSections + 1; j++)
{
//now read number of patches, then how many vertex's
int patches = br.ReadInt32();
for (int k = 0; k < patches; k++)
{
section[j].triangleList = new List <vec2>();
section[j].patchList.Add(section[j].triangleList);
int verts = br.ReadInt32();
for (int v = 0; v < verts; v++)
{
vecFix.x = br.ReadDouble();
vecFix.z = br.ReadDouble();
section[j].triangleList.Add(vecFix);
}
}
}
s = br.ReadString();
if (s.IndexOf("$TotalSqM") == -1)
{
MessageBox.Show("Meters header is Corrupt"); JobClose(); return;
}
totalSquareMeters = br.ReadDouble();//total square meters
}
// Contour points ----------------------------------------------------------------------------
fileAndDirectory = workingDirectory + currentFieldDirectory + "\\Contour.ctr";
if (!File.Exists(fileAndDirectory))
{
MessageBox.Show("Missing Contour File");
return;
}
/*
* $Contour
* First_2017Feb10_09.58.51.AM
* //$Offsets
* //533210,5927965,12
* $Patches
* 2
* 76
* 1.384,3.135,-86.304,0 -easting, heading, northing, altitude
*/
using (BinaryReader br = new BinaryReader(new FileStream(fileAndDirectory, FileMode.Open)))
{
string s = br.ReadString(); //$Contour string
s = br.ReadString(); //current directory string
//Offset header
s = br.ReadString(); //$Offsets
//read the Offsets - Just read and skip them
br.ReadInt32();
br.ReadInt32();
br.ReadDouble();
//$Patches line
s = br.ReadString();
if (s.IndexOf("$Patches") == -1)
{
MessageBox.Show("Contour File is Corrupt"); return;
}
vec4 vecFix = new vec4(0, 0, 0, 0);
//now read number of patches, then how many vertex's
int patches = br.ReadInt32();
for (int k = 0; k < patches; k++)
{
ct.ptList = new List <vec4>();
ct.stripList.Add(ct.ptList);
int verts = br.ReadInt32();
for (int v = 0; v < verts; v++)
{
vecFix.x = br.ReadDouble();
vecFix.y = br.ReadDouble();
vecFix.z = br.ReadDouble();
vecFix.k = br.ReadDouble();
ct.ptList.Add(vecFix);
}
}
}
// Flags -------------------------------------------------------------------------------------------------
//Either exit or update running save
fileAndDirectory = workingDirectory + currentFieldDirectory + "\\Flags.flg";
if (!File.Exists(fileAndDirectory))
{
MessageBox.Show("Missing Flag File");
return;
}
using (BinaryReader br = new BinaryReader(new FileStream(fileAndDirectory, FileMode.Open)))
{
string s = br.ReadString();
s = br.ReadString();
//Offset header
s = br.ReadString(); //$Offsets
//read the Offsets - Just read and skip them
br.ReadInt32();
br.ReadInt32();
br.ReadDouble();
//CFlag flagPt = new CFlag(0,0,0,0,0);
int points = br.ReadInt32();
if (points > 0)
{
double lat;
double longi;
double east;
double nort;
int color, ID;
for (int v = 0; v < points; v++)
{
lat = br.ReadDouble();
longi = br.ReadDouble();
east = br.ReadDouble();
nort = br.ReadDouble();
color = br.ReadInt32();
ID = br.ReadInt32();
CFlag flagPt = new CFlag(lat, longi, east, nort, color, ID);
flagPts.Add(flagPt);
}
}
}
}//end of open file
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;
}
}
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);
}
}
private void btnInField_Click(object sender, EventArgs e)
{
string infieldList = "";
int numFields = 0;
string[] dirs = Directory.GetDirectories(mf.fieldsDirectory);
foreach (string dir in dirs)
{
double northingOffset = 0;
double eastingOffset = 0;
string fieldDirectory = Path.GetFileName(dir);
string filename = dir + "\\Field.txt";
string line;
//make sure directory has a field.txt in it
if (File.Exists(filename))
{
using (StreamReader reader = new StreamReader(filename))
{
try
{
//Date time line
for (int i = 0; i < 4; i++)
{
line = reader.ReadLine();
}
//start positions
if (!reader.EndOfStream)
{
line = reader.ReadLine();
string[] offs = line.Split(',');
eastingOffset = (double.Parse(offs[0], CultureInfo.InvariantCulture));
northingOffset = (double.Parse(offs[1], CultureInfo.InvariantCulture));
}
}
catch (Exception)
{
var form = new FormTimedMessage(2000, gStr.gsFieldFileIsCorrupt, gStr.gsChooseADifferentField);
}
}
//grab the boundary
filename = dir + "\\Boundary.txt";
if (File.Exists(filename))
{
List <vec3> pointList = new List <vec3>();
using (StreamReader reader = new StreamReader(filename))
{
try
{
//read header
line = reader.ReadLine(); //Boundary
if (!reader.EndOfStream) //empty boundary field
{
//True or False OR points from older boundary files
line = reader.ReadLine();
//Check for older boundary files, then above line string is num of points
if (line == "True" || line == "False")
{
line = reader.ReadLine(); //number of points
}
//Check for latest boundary files, then above line string is num of points
if (line == "True" || line == "False")
{
line = reader.ReadLine(); //number of points
}
int numPoints = int.Parse(line);
vec2[] linePoints = new vec2[numPoints];
if (numPoints > 0)
{
//load the line
for (int i = 0; i < numPoints; i++)
{
line = reader.ReadLine();
string[] words = line.Split(',');
vec2 vecPt = new vec2(
double.Parse(words[0], CultureInfo.InvariantCulture) + eastingOffset,
double.Parse(words[1], CultureInfo.InvariantCulture) + northingOffset);
linePoints[i] = vecPt;
}
int j = linePoints.Length - 1;
bool oddNodes = false;
double x = mf.pn.fix.easting;
double y = mf.pn.fix.northing;
for (int i = 0; i < linePoints.Length; i++)
{
if ((linePoints[i].northing < y && linePoints[j].northing >= y ||
linePoints[j].northing < y && linePoints[i].northing >= y) &&
(linePoints[i].easting <= x || linePoints[j].easting <= x))
{
oddNodes ^= (linePoints[i].easting + (y - linePoints[i].northing) /
(linePoints[j].northing - linePoints[i].northing) * (linePoints[j].easting - linePoints[i].easting) < x);
}
j = i;
}
if (oddNodes)
{
numFields++;
if (string.IsNullOrEmpty(infieldList))
{
infieldList += Path.GetFileName(dir);
}
else
{
infieldList += "," + Path.GetFileName(dir);
}
}
}
}
}
catch (Exception)
{
}
}
}
}
}
if (!string.IsNullOrEmpty(infieldList))
{
mf.filePickerFileAndDirectory = "";
if (numFields > 1)
{
using (var form = new FormDrivePicker(mf, infieldList))
{
var result = form.ShowDialog();
//returns full field.txt file dir name
if (result == DialogResult.Yes)
{
mf.FileOpenField(mf.filePickerFileAndDirectory);
Close();
}
else
{
return;
}
}
}
else // 1 field found
{
mf.filePickerFileAndDirectory = mf.fieldsDirectory + infieldList + "\\Field.txt";
mf.FileOpenField(mf.filePickerFileAndDirectory);
Close();
}
}
else //no fields found
{
var form2 = new FormTimedMessage(2000, gStr.gsNoFieldsFound, gStr.gsFieldNotOpen);
form2.Show();
}
}
public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
{
//determine closest point
double minDistance = 9999999;
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));
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;
}
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
howManyPathsAway = Math.Round(minDistance / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
//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);
double abHeading = curList[A].heading;
//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 = (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;
}
}
private void btnStop_Click(object sender, EventArgs e)
{
//go make the list
mf.yt.isRecordingCustomYouTurn = false;
//first one is the reference the rest are subtracted from, remove it.
mf.yt.youFileList.RemoveAt(0);
int numShapePoints = mf.yt.youFileList.Count;
int i;
vec2[] pt = new vec2[numShapePoints];
//put the driven list into an array
for (i = 0; i < numShapePoints; i++)
{
pt[i].easting = mf.yt.youFileList[i].easting;
pt[i].northing = mf.yt.youFileList[i].northing;
}
//empty out the youFileList
mf.yt.youFileList.Clear();
//rotate pattern to match AB Line heading
for (i = 0; i < pt.Length; i++)
{
//since we want to unwind the heading, we go not negative for heading unlike GPS circle
double xr, yr;
xr = (Math.Cos(mf.ABLine.abHeading) * pt[i].easting) - (Math.Sin(mf.ABLine.abHeading) * pt[i].northing);
yr = (Math.Sin(mf.ABLine.abHeading) * pt[i].easting) + (Math.Cos(mf.ABLine.abHeading) * pt[i].northing);
//update the array
pt[i].easting = xr;
pt[i].northing = yr;
}
//scale the drawing to match exactly the ABLine width
double adjustFactor = pt[pt.Length - 1].easting;
adjustFactor = (mf.tool.toolWidth - mf.tool.toolOverlap + mf.tool.toolOffset) / adjustFactor;
for (i = 0; i < pt.Length; i++)
{
pt[i].easting *= adjustFactor;
pt[i].northing *= adjustFactor;
}
// 2nd pass scale it so coords are based on 10m
//last point is the width
adjustFactor = pt[pt.Length - 1].easting;
adjustFactor = 10.0 / adjustFactor;
for (i = 0; i < pt.Length; i++)
{
pt[i].easting *= adjustFactor;
pt[i].northing *= adjustFactor;
mf.yt.youFileList.Add(pt[i]);
}
//create the file.
string dir = System.Reflection.Assembly.GetExecutingAssembly().Location;
dir = System.IO.Path.GetDirectoryName(dir);
dir += @"/Dependencies/YouTurnShapes/Custom.txt";
using (StreamWriter writer = new StreamWriter(dir))
{
writer.WriteLine(pt.Length);
for (i = 0; i < mf.yt.youFileList.Count; i++)
{
writer.WriteLine(mf.yt.youFileList[i].easting + "," + mf.yt.youFileList[i].northing);
}
}
Close();
}
//calculate the extreme tool left, right velocities, each section lookahead, and whether or not its going backwards
public void CalculateSectionLookAhead(double northing, double easting, double cosHeading, double sinHeading)
{
//calculate left side of section 1
vec2 left = new vec2(0, 0);
vec2 right = left;
double leftSpeed = 0, rightSpeed = 0, leftLook = 0, rightLook = 0;
//now loop all the section rights and the one extreme left
for (int j = 0; j < vehicle.numOfSections; j++)
{
if (j == 0)
{
//only one first left point, the rest are all rights moved over to left
section[j].leftPoint = new vec2(cosHeading * (section[j].positionLeft) + easting,
sinHeading * (section[j].positionLeft) + northing);
left = section[j].leftPoint - section[j].lastLeftPoint;
//save a copy for next time
section[j].lastLeftPoint = section[j].leftPoint;
//get the speed for left side only once
leftSpeed = left.GetLength() / fixUpdateTime * 10;
leftLook = leftSpeed * vehicle.toolLookAhead;
//save the far left speed
vehicle.toolFarLeftSpeed = leftSpeed;
}
else
{
//right point from last section becomes this left one
section[j].leftPoint = section[j-1].rightPoint;
left = section[j].leftPoint - section[j].lastLeftPoint;
//save a copy for next time
section[j].lastLeftPoint = section[j].leftPoint;
leftSpeed = rightSpeed;
}
section[j].rightPoint = new vec2(cosHeading * (section[j].positionRight) + easting,
sinHeading * (section[j].positionRight) + northing);
//now we have left and right for this section
right = section[j].rightPoint - section[j].lastRightPoint;
//save a copy for next time
section[j].lastRightPoint = section[j].rightPoint;
//grab vector length and convert to meters/sec/10 pixels per meter
rightSpeed = right.GetLength() / fixUpdateTime * 10;
rightLook = rightSpeed * vehicle.toolLookAhead;
//Is section outer going forward or backward
double head = left.HeadingXZ();
if (Math.PI - Math.Abs(Math.Abs(head - fixHeadingSection) - Math.PI) > glm.PIBy2) leftLook *= -1;
head = right.HeadingXZ();
if (Math.PI - Math.Abs(Math.Abs(head - fixHeadingSection) - Math.PI) > glm.PIBy2) rightLook *= -1;
//choose fastest speed
if (leftLook > rightLook) section[j].sectionLookAhead = leftLook;
else section[j].sectionLookAhead = rightLook;
if (section[j].sectionLookAhead > 190) section[j].sectionLookAhead = 190;
//Doing the slow mo, exceeding buffer so just set as minimum 0.5 meter
if (currentStepFix >= totalFixSteps - 1) section[j].sectionLookAhead = 5;
}//endfor
//set up the super for youturn
section[vehicle.numOfSections].isInsideBoundary = true;
//determine if section is in boundary using the section left/right positions
for (int j = 0; j < vehicle.numOfSections; j++)
{
if (boundary.isSet)
{
bool isLeftIn = true, isRightIn = true;
if (j == 0)
{
//only one first left point, the rest are all rights moved over to left
isLeftIn = boundary.IsPrePointInPolygon(section[j].leftPoint);
isRightIn = boundary.IsPrePointInPolygon(section[j].rightPoint);
if (isLeftIn && isRightIn) section[j].isInsideBoundary = true;
else section[j].isInsideBoundary = false;
}
else
{
//grab the right of previous section, its the left of this section
isLeftIn = isRightIn;
isRightIn = boundary.IsPrePointInPolygon(section[j].rightPoint);
if (isLeftIn && isRightIn) section[j].isInsideBoundary = true;
else section[j].isInsideBoundary = false;
}
section[vehicle.numOfSections].isInsideBoundary &= section[j].isInsideBoundary;
}
//no boundary created so always inside
else
{
section[j].isInsideBoundary = true;
section[vehicle.numOfSections].isInsideBoundary = false;
}
}
//with left and right tool velocity to determine rate of triangle generation, corners are more
//save far right speed, 0 if going backwards, in meters/sec
if (section[vehicle.numOfSections - 1].sectionLookAhead > 0) vehicle.toolFarRightSpeed = rightSpeed * 0.05;
else vehicle.toolFarRightSpeed = 0;
//save left side, 0 if going backwards, in meters/sec convert back from pixels/m
if (section[0].sectionLookAhead > 0) vehicle.toolFarLeftSpeed = vehicle.toolFarLeftSpeed * 0.05;
else vehicle.toolFarLeftSpeed = 0;
}
public void FixGeoFenceLine(double totalHeadWidth, List <CBndPt> curBnd, double spacing)
{
//count the points from the boundary
int lineCount = geoFenceLine.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], geoFenceLine[j]);
if (distance < (totalHeadWidth * 0.96))
{
geoFenceLine.RemoveAt(j);
lineCount = geoFenceLine.Count;
j = -1;
}
}
}
//make sure distance isn't too small between points on turnLine
bndCount = geoFenceLine.Count;
//double spacing = mf.vehicle.toolWidth * 0.25;
for (int i = 0; i < bndCount - 1; i++)
{
distance = glm.Distance(geoFenceLine[i], geoFenceLine[i + 1]);
if (distance < spacing)
{
geoFenceLine.RemoveAt(i + 1);
bndCount = geoFenceLine.Count;
i--;
}
}
//make sure distance isn't too big between points on Turn
bndCount = geoFenceLine.Count;
for (int i = 0; i < bndCount; i++)
{
int j = i + 1;
if (j == bndCount)
{
j = 0;
}
distance = glm.Distance(geoFenceLine[i], geoFenceLine[j]);
if (distance > (spacing * 1.25))
{
vec2 pointB = new vec2((geoFenceLine[i].easting + geoFenceLine[j].easting) / 2.0, (geoFenceLine[i].northing + geoFenceLine[j].northing) / 2.0);
geoFenceLine.Insert(j, pointB);
bndCount = geoFenceLine.Count;
i--;
}
}
//make sure headings are correct for calculated points
//CalculateTurnHeadings();
}
public OneDubinsPath(double length1, double length2, double length3, vec2 tangent1, vec2 tangent2, PathType pathType)
{
//Calculate the total length of this path
this.totalLength = length1 + length2 + length3;
this.length1 = length1;
this.length2 = length2;
this.length3 = length3;
this.tangent1 = tangent1;
this.tangent2 = tangent2;
this.pathType = pathType;
}
public void FindClosestBoundaryPoint(vec2 fromPt)
{
boxA.easting = fromPt.easting - (Math.Sin(mf.fixHeading + glm.PIBy2) * mf.vehicle.toolWidth * 0.5);
boxA.northing = fromPt.northing - (Math.Cos(mf.fixHeading + glm.PIBy2) * mf.vehicle.toolWidth * 0.5);
boxB.easting = fromPt.easting + (Math.Sin(mf.fixHeading + glm.PIBy2) * mf.vehicle.toolWidth * 0.5);
boxB.northing = fromPt.northing + (Math.Cos(mf.fixHeading + glm.PIBy2) * mf.vehicle.toolWidth * 0.5);
boxC.easting = boxB.easting + (Math.Sin(mf.fixHeading) * 2000.0);
boxC.northing = boxB.northing + (Math.Cos(mf.fixHeading) * 2000.0);
boxD.easting = boxA.easting + (Math.Sin(mf.fixHeading) * 2000.0);
boxD.northing = boxA.northing + (Math.Cos(mf.fixHeading) * 2000.0);
//determine if point is inside bounding box
bdList.Clear();
int ptCount = ptList.Count;
for (int p = 0; p < ptCount; p++)
{
if ((((boxB.easting - boxA.easting) * (ptList[p].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (ptList[p].easting - boxA.easting))) < 0)
{
continue;
}
if ((((boxD.easting - boxC.easting) * (ptList[p].northing - boxC.northing))
- ((boxD.northing - boxC.northing) * (ptList[p].easting - boxC.easting))) < 0)
{
continue;
}
if ((((boxC.easting - boxB.easting) * (ptList[p].northing - boxB.northing))
- ((boxC.northing - boxB.northing) * (ptList[p].easting - boxB.easting))) < 0)
{
continue;
}
if ((((boxA.easting - boxD.easting) * (ptList[p].northing - boxD.northing))
- ((boxA.northing - boxD.northing) * (ptList[p].easting - boxD.easting))) < 0)
{
continue;
}
//it's in the box, so add to list
closestBoundaryPt.easting = ptList[p].easting;
closestBoundaryPt.northing = ptList[p].northing;
bdList.Add(closestBoundaryPt);
}
//which of the points is closest
closestBoundaryPt.easting = -1; closestBoundaryPt.northing = -1;
ptCount = bdList.Count;
if (ptCount == 0)
{
return;
}
else
{
//determine closest point
double minDistance = 9999999;
for (int i = 0; i < ptCount; i++)
{
double dist = ((fromPt.easting - bdList[i].easting) * (fromPt.easting - bdList[i].easting))
+ ((fromPt.northing - bdList[i].northing) * (fromPt.northing - bdList[i].northing));
if (minDistance >= dist)
{
minDistance = dist;
closestBoundaryPt = bdList[i];
}
}
}
}
