示例#1
0
        //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
        }
示例#3
0
        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");
        }
示例#4
0
        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";
            }
        }
示例#5
0
        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;
            }
        }
示例#6
0
文件: CGLM.cs 项目: BenjaminK87/v4202
 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)));
 }
示例#7
0
        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();
            }
        }
示例#8
0
        //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);



            }
        }
示例#9
0
        public void GetCurrentABLine(vec3 pivot)
        {
            //move the ABLine over based on the overlap amount set in vehicle
            double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;

            //x2-x1
            double dx = refABLineP2.easting - refABLineP1.easting;
            //z2-z1
            double dy = refABLineP2.northing - refABLineP1.northing;

            //how far are we away from the reference line at 90 degrees
            distanceFromRefLine = ((dy * pivot.easting) - (dx * pivot.northing) + (refABLineP2.easting
                                                                                   * refABLineP1.northing) - (refABLineP2.northing * refABLineP1.easting))
                                  / Math.Sqrt((dy * dy) + (dx * dx));

            //sign of distance determines which side of line we are on
            if (distanceFromRefLine > 0)
            {
                refLineSide = 1;
            }
            else
            {
                refLineSide = -1;
            }

            //absolute the distance
            distanceFromRefLine = Math.Abs(distanceFromRefLine);

            //Which ABLine is the vehicle on, negative is left and positive is right side
            howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);

            //generate that pass number as signed integer
            passNumber = Convert.ToInt32(refLineSide * howManyPathsAway);

            //calculate the new point that is number of implement widths over
            double toolOffset = mf.vehicle.toolOffset;
            vec2   point1;

            //depending which way you are going, the offset can be either side
            if (isABSameAsVehicleHeading)
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing);
            }
            else
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing);
            }

            //create the new line extent points for current ABLine based on original heading of AB line
            currentABLineP1.easting  = point1.easting - (Math.Sin(abHeading) * 40000.0);
            currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0);

            currentABLineP2.easting  = point1.easting + (Math.Sin(abHeading) * 40000.0);
            currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0);

            //get the distance from currently active AB line
            //x2-x1
            dx = currentABLineP2.easting - currentABLineP1.easting;
            //z2-z1
            dy = currentABLineP2.northing - currentABLineP1.northing;

            //save a copy of dx,dy in youTurn
            mf.yt.dxAB = dx; mf.yt.dyAB = dy;

            //how far from current AB Line is fix
            distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
                                                                                       * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
                                      / Math.Sqrt((dy * dy) + (dx * dx));

            //are we on the right side or not
            isOnRightSideCurrentLine = distanceFromCurrentLine > 0;


            // 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;
            }
        }
示例#10
0
        //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();
            }
        }
示例#11
0
        //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;
            }
        }
示例#12
0
        //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]);
            }
        }
示例#13
0
        //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();
            }
        }
示例#14
0
        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";
                }
            }
        }
示例#15
0
        //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
        }
示例#16
0
        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;
            }
        }
示例#17
0
文件: CGLM.cs 项目: BenjaminK87/v4202
 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)));
 }
示例#18
0
        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;
            }
        }
示例#19
0
文件: CGLM.cs 项目: BenjaminK87/v4202
 public static double DistanceSquared(vec2 first, vec2 second)
 {
     return(
         Math.Pow(first.easting - second.easting, 2)
         + Math.Pow(first.northing - second.northing, 2));
 }
示例#20
0
        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);
                            }
                        }
                    }
                }
            }
        }
示例#21
0
        }//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
示例#22
0
        public void GetCurrentABLine(vec3 pivot)
        {
            //move the ABLine over based on the overlap amount set in vehicle
            double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;

            //x2-x1
            double dx = refABLineP2.easting - refABLineP1.easting;
            //z2-z1
            double dy = refABLineP2.northing - refABLineP1.northing;

            //how far are we away from the reference line at 90 degrees
            distanceFromRefLine = ((dy * pivot.easting) - (dx * pivot.northing) + (refABLineP2.easting
                                                                                   * refABLineP1.northing) - (refABLineP2.northing * refABLineP1.easting))
                                  / Math.Sqrt((dy * dy) + (dx * dx));

            //sign of distance determines which side of line we are on
            if (distanceFromRefLine > 0)
            {
                refLineSide = 1;
            }
            else
            {
                refLineSide = -1;
            }

            //absolute the distance
            distanceFromRefLine = Math.Abs(distanceFromRefLine);

            //Which ABLine is the vehicle on, negative is left and positive is right side
            howManyPathsAway = Math.Round(distanceFromRefLine / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);

            //generate that pass number as signed integer
            passNumber = Convert.ToInt32(refLineSide * howManyPathsAway);

            //calculate the new point that is number of implement widths over
            double toolOffset = mf.vehicle.toolOffset;
            vec2   point1;

            //depending which way you are going, the offset can be either side
            if (isABSameAsVehicleHeading)
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) - toolOffset)) + refPoint1.northing);
            }
            else
            {
                point1 = new vec2((Math.Cos(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.easting,
                                  (Math.Sin(-abHeading) * ((widthMinusOverlap * howManyPathsAway * refLineSide) + toolOffset)) + refPoint1.northing);
            }

            //create the new line extent points for current ABLine based on original heading of AB line
            currentABLineP1.easting  = point1.easting - (Math.Sin(abHeading) * 40000.0);
            currentABLineP1.northing = point1.northing - (Math.Cos(abHeading) * 40000.0);

            currentABLineP2.easting  = point1.easting + (Math.Sin(abHeading) * 40000.0);
            currentABLineP2.northing = point1.northing + (Math.Cos(abHeading) * 40000.0);

            //get the distance from currently active AB line
            //x2-x1
            dx = currentABLineP2.easting - currentABLineP1.easting;
            //z2-z1
            dy = currentABLineP2.northing - currentABLineP1.northing;

            //save a copy of dx,dy in youTurn
            mf.yt.dxAB = dx; mf.yt.dyAB = dy;

            //how far from current AB Line is fix
            distanceFromCurrentLine = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
                                                                                       * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
                                      / Math.Sqrt((dy * dy) + (dx * dx));

            //are we on the right side or not
            isOnRightSideCurrentLine = distanceFromCurrentLine > 0;

            //absolute the distance
            distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);

            //update base on autosteer settings and distance from line
            double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine);

            mf.lookaheadActual = goalPointDistance;

            //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
            abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
            if (abFixHeadingDelta >= Math.PI)
            {
                abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
            }

            // ** Pure pursuit ** - calc point on ABLine closest to current position
            double U = (((pivot.easting - currentABLineP1.easting) * dx)
                        + ((pivot.northing - currentABLineP1.northing) * dy))
                       / ((dx * dx) + (dy * dy));

            //point on AB line closest to pivot axle point
            rEastAB  = currentABLineP1.easting + (U * dx);
            rNorthAB = currentABLineP1.northing + (U * dy);

            if (abFixHeadingDelta >= glm.PIBy2)
            {
                isABSameAsVehicleHeading = false;
                goalPointAB.easting      = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
                goalPointAB.northing     = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
            }
            else
            {
                isABSameAsVehicleHeading = true;
                goalPointAB.easting      = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
                goalPointAB.northing     = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
            }

            //calc "D" the distance from pivot axle to lookahead point
            double goalPointDistanceDSquared
                = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);

            //calculate the the new x in local coordinates and steering angle degrees based on wheelbase
            double localHeading = glm.twoPI - mf.fixHeading;

            ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                           + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));

            steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                        + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
                                                   / goalPointDistanceDSquared));
            if (steerAngleAB < -mf.vehicle.maxSteerAngle)
            {
                steerAngleAB = -mf.vehicle.maxSteerAngle;
            }
            if (steerAngleAB > mf.vehicle.maxSteerAngle)
            {
                steerAngleAB = mf.vehicle.maxSteerAngle;
            }

            //limit circle size for display purpose
            if (ppRadiusAB < -500)
            {
                ppRadiusAB = -500;
            }
            if (ppRadiusAB > 500)
            {
                ppRadiusAB = 500;
            }

            radiusPointAB.easting  = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
            radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));

            //Convert to millimeters
            distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);

            //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
            angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;

            //clamp the steering angle to not exceed safe angular velocity
            if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
            {
                steerAngleAB = glm.toDegrees(steerAngleAB > 0 ? (Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity)
                                                                           / (glm.twoPI * mf.pn.speed * 0.277777)))
                    : (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
            }

            //distance is negative if on left, positive if on right
            if (isABSameAsVehicleHeading)
            {
                if (!isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }
            }

            //opposite way so right is left
            else
            {
                if (isOnRightSideCurrentLine)
                {
                    distanceFromCurrentLine *= -1.0;
                }
            }

            mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
            mf.guidanceLineSteerAngle  = (Int16)(steerAngleAB * 100);

            if (mf.yt.isYouTurnTriggered)
            {
                //do the pure pursuit from youTurn
                mf.yt.DistanceFromYouTurnLine();

                mf.seq.DoSequenceEvent();

                //now substitute what it thinks are AB line values with auto turn values
                steerAngleAB            = mf.yt.steerAngleYT;
                distanceFromCurrentLine = mf.yt.distanceFromCurrentLine;

                goalPointAB            = mf.yt.goalPointYT;
                radiusPointAB.easting  = mf.yt.radiusPointYT.easting;
                radiusPointAB.northing = mf.yt.radiusPointYT.northing;
                ppRadiusAB             = mf.yt.ppRadiusYT;
            }
        }
示例#23
0
        public void GetCurrentABLine(vec3 pivot, vec3 steer)
        {
            double dx, dy;

            //build new current ref line if required
            if (!isABValid || ((mf.secondsSinceStart - lastSecond) > 0.66 && (!mf.isAutoSteerBtnOn || mf.mc.steerSwitchHigh)))
            {
                BuildCurrentABLineList(pivot);
            }

            //Check uturn first
            if (mf.yt.isYouTurnTriggered && mf.yt.DistanceFromYouTurnLine())//do the pure pursuit from youTurn
            {
                //now substitute what it thinks are AB line values with auto turn values
                steerAngleAB = mf.yt.steerAngleYT;
                distanceFromCurrentLinePivot = mf.yt.distanceFromCurrentLine;

                goalPointAB            = mf.yt.goalPointYT;
                radiusPointAB.easting  = mf.yt.radiusPointYT.easting;
                radiusPointAB.northing = mf.yt.radiusPointYT.northing;
                ppRadiusAB             = mf.yt.ppRadiusYT;
            }

            //Stanley
            else if (mf.isStanleyUsed)
            {
                mf.gyd.StanleyGuidanceABLine(currentABLineP1, currentABLineP2, pivot, steer);
            }

            //Pure Pursuit
            else
            {
                //get the distance from currently active AB line
                //x2-x1
                dx = currentABLineP2.easting - currentABLineP1.easting;
                //z2-z1
                dy = currentABLineP2.northing - currentABLineP1.northing;

                //save a copy of dx,dy in youTurn
                mf.yt.dxAB = dx; mf.yt.dyAB = dy;

                //how far from current AB Line is fix
                distanceFromCurrentLinePivot = ((dy * pivot.easting) - (dx * pivot.northing) + (currentABLineP2.easting
                                                                                                * currentABLineP1.northing) - (currentABLineP2.northing * currentABLineP1.easting))
                                               / Math.Sqrt((dy * dy) + (dx * dx));

                //integral slider is set to 0
                if (mf.vehicle.purePursuitIntegralGain != 0 && !mf.isReverse)
                {
                    pivotDistanceError = distanceFromCurrentLinePivot * 0.2 + pivotDistanceError * 0.8;

                    if (counter2++ > 4)
                    {
                        pivotDerivative        = pivotDistanceError - pivotDistanceErrorLast;
                        pivotDistanceErrorLast = pivotDistanceError;
                        counter2         = 0;
                        pivotDerivative *= 2;

                        //limit the derivative
                        //if (pivotDerivative > 0.03) pivotDerivative = 0.03;
                        //if (pivotDerivative < -0.03) pivotDerivative = -0.03;
                        //if (Math.Abs(pivotDerivative) < 0.01) pivotDerivative = 0;
                    }

                    //pivotErrorTotal = pivotDistanceError + pivotDerivative;

                    if (mf.isAutoSteerBtnOn &&
                        Math.Abs(pivotDerivative) < (0.1) &&
                        mf.avgSpeed > 2.5 &&
                        !mf.yt.isYouTurnTriggered)
                    //&& Math.Abs(pivotDistanceError) < 0.2)

                    {
                        //if over the line heading wrong way, rapidly decrease integral
                        if ((inty < 0 && distanceFromCurrentLinePivot < 0) || (inty > 0 && distanceFromCurrentLinePivot > 0))
                        {
                            inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.04;
                        }
                        else
                        {
                            if (Math.Abs(distanceFromCurrentLinePivot) > 0.02)
                            {
                                inty += pivotDistanceError * mf.vehicle.purePursuitIntegralGain * -0.02;
                                if (inty > 0.2)
                                {
                                    inty = 0.2;
                                }
                                else if (inty < -0.2)
                                {
                                    inty = -0.2;
                                }
                            }
                        }
                    }
                    else
                    {
                        inty *= 0.95;
                    }
                }
                else
                {
                    inty = 0;
                }

                //Subtract the two headings, if > 1.57 its going the opposite heading as refAB
                abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
                if (abFixHeadingDelta >= Math.PI)
                {
                    abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
                }

                // ** Pure pursuit ** - calc point on ABLine closest to current position
                double U = (((pivot.easting - currentABLineP1.easting) * dx)
                            + ((pivot.northing - currentABLineP1.northing) * dy))
                           / ((dx * dx) + (dy * dy));

                //point on AB line closest to pivot axle point
                rEastAB  = currentABLineP1.easting + (U * dx);
                rNorthAB = currentABLineP1.northing + (U * dy);

                //update base on autosteer settings and distance from line
                double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();

                if (mf.isReverse ? isHeadingSameWay : !isHeadingSameWay)
                {
                    goalPointAB.easting  = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
                    goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
                }
                else
                {
                    goalPointAB.easting  = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
                    goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
                }

                //calc "D" the distance from pivot axle to lookahead point
                double goalPointDistanceDSquared
                    = glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);

                //calculate the the new x in local coordinates and steering angle degrees based on wheelbase
                double localHeading;

                if (isHeadingSameWay)
                {
                    localHeading = glm.twoPI - mf.fixHeading + inty;
                }
                else
                {
                    localHeading = glm.twoPI - mf.fixHeading - inty;
                }

                ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                               + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));

                steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
                                                            + ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
                                                       / goalPointDistanceDSquared));

                if (mf.ahrs.imuRoll != 88888)
                {
                    steerAngleAB += mf.ahrs.imuRoll * -mf.gyd.sideHillCompFactor;
                }

                if (steerAngleAB < -mf.vehicle.maxSteerAngle)
                {
                    steerAngleAB = -mf.vehicle.maxSteerAngle;
                }
                if (steerAngleAB > mf.vehicle.maxSteerAngle)
                {
                    steerAngleAB = mf.vehicle.maxSteerAngle;
                }

                //limit circle size for display purpose
                if (ppRadiusAB < -500)
                {
                    ppRadiusAB = -500;
                }
                if (ppRadiusAB > 500)
                {
                    ppRadiusAB = 500;
                }

                radiusPointAB.easting  = pivot.easting + (ppRadiusAB * Math.Cos(localHeading));
                radiusPointAB.northing = pivot.northing + (ppRadiusAB * Math.Sin(localHeading));

                if (mf.isAngVelGuidance)
                {
                    //angular velocity in rads/sec  = 2PI * m/sec * radians/meters
                    mf.setAngVel = 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleAB))) / mf.vehicle.wheelbase;
                    mf.setAngVel = glm.toDegrees(mf.setAngVel) * 100;

                    //clamp the steering angle to not exceed safe angular velocity
                    if (Math.Abs(mf.setAngVel) > 1000)
                    {
                        //mf.setAngVel = mf.setAngVel < 0 ? -mf.vehicle.maxAngularVelocity : mf.vehicle.maxAngularVelocity;
                        mf.setAngVel = mf.setAngVel < 0 ? -1000 : 1000;
                    }
                }

                //distance is negative if on left, positive if on right
                if (!isHeadingSameWay)
                {
                    distanceFromCurrentLinePivot *= -1.0;
                }

                //Convert to millimeters
                mf.guidanceLineDistanceOff = (short)Math.Round(distanceFromCurrentLinePivot * 1000.0, MidpointRounding.AwayFromZero);
                mf.guidanceLineSteerAngle  = (short)(steerAngleAB * 100);
            }
        }
示例#24
0
        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();
            }
        }
示例#25
0
        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;
            }
        }
示例#26
0
        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();
        }
示例#27
0
        //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;                
        }
示例#28
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();
        }
示例#29
0
        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;
        }
示例#30
0
        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];
                    }
                }
            }
        }