Ejemplo n.º 1
0
        public void GeneratePathToStartInfield()
        {
            //go to the startpoint
            CDubins dubPathToStart = new CDubins();

            CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;

            ////get the dubins path vec3 point coordinates of turn
            //dubList.Clear();
            //dubList = dubPathToStart.GenerateDubins(endHeadlandPoint, startInfieldPoint, mf.bnd);

            //transfer point list to genList class
            for (int i = 0; i < dubList.Count; i++)
            {
                CGenPathPt pt = new CGenPathPt(dubList[i].easting, dubList[i].northing, dubList[i].heading);
                if (i < 10)
                {
                    pt.sequence = 1;
                }
                genList.Add(pt);
            }

            //made the path so set the flag
            isPathToInfieldMade = true;

            genListCount        = genList.Count;
            currentPositonIndex = 0;
            isEndOfTheGenLine   = false;
        }
Ejemplo n.º 2
0
        public void GeneratePathHome()
        {
            genList.Clear();
            currentPositonIndex = 0;
            isEndOfTheGenLine   = false;

            //go to the startpoint
            CDubins dubPathToStart = new CDubins();

            CDubins.turningRadius = mf.vehicle.minTurningRadius * 2.0;

            ////get the dubins path vec3 point coordinates of turn
            //dubList.Clear();
            //dubList = dubPathToStart.GenerateDubins(mf.pivotAxlePos, homePoint, mf.bnd);

            //transfer point list to genList class
            for (int i = 0; i < dubList.Count; i++)
            {
                CGenPathPt pt = new CGenPathPt(dubList[i].easting, dubList[i].northing, dubList[i].heading);
                genList.Add(pt);
            }

            genListCount        = genList.Count;
            currentPositonIndex = 0;

            //shut everything off
            isDrivingHome = true;
        }
Ejemplo n.º 3
0
        private void GetDubinsPath(vec3 goal)
        {
            CDubins dubPath = new CDubins();

            //
            pivotPos = mf.pivotAxlePos;

            //bump it forward
            vec3 pt2 = new vec3
            {
                easting  = pivotPos.easting + (Math.Sin(pivotPos.heading) * 4),
                northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 4),
                heading  = pivotPos.heading
            };

            //get the dubins path vec3 point coordinates of turn
            dubinsList.Clear();
            dubinsList = dubPath.GenerateDubins(pt2, goal, mf.turn);

            dubinsList.Insert(0, mf.pivotAxlePos);

            //transfer point list to recPath class point style
            dubList.Clear();
            for (int i = 0; i < dubinsList.Count; i++)
            {
                CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
                dubList.Add(pt);
            }

            //no longer needed
            dubinsList?.Clear();
        }
Ejemplo n.º 4
0
        private void MakeDubinsLineFromPivotToFlag()
        {
            //if (mf.ABLine.isBtnABLineOn)
            //{
            //    mf.ABLine.isBtnABLineOn = false;
            //    mf.btnABLine.Image = Properties.Resources.ABLineOff;
            //}

            CDubins.turningRadius = mf.vehicle.minTurningRadius * 3.0;
            CDubins dubPath = new CDubins();

            // current psition
            vec3 steerAxlePosRP = mf.pivotAxlePos;

            //bump it back so you can line up to point
            vec3 goal = new vec3
            {
                easting  = mf.flagPts[mf.flagNumberPicked - 1].easting - (Math.Sin(mf.flagPts[mf.flagNumberPicked - 1].heading) * 6),
                northing = mf.flagPts[mf.flagNumberPicked - 1].northing - (Math.Cos(mf.flagPts[mf.flagNumberPicked - 1].heading) * 6),
                heading  = mf.flagPts[mf.flagNumberPicked - 1].heading
            };

            //bump it forward
            vec3 pt2 = new vec3
            {
                easting  = steerAxlePosRP.easting + (Math.Sin(steerAxlePosRP.heading) * 6),
                northing = steerAxlePosRP.northing + (Math.Cos(steerAxlePosRP.heading) * 6),
                heading  = steerAxlePosRP.heading
            };
        }
Ejemplo n.º 5
0
        private void GetDubinsPath(vec3 goal)
        {
            CDubins dubPath = new CDubins();

            //get the dubins path vec3 point coordinates of turn
            dubinsList.Clear();
            dubinsList = dubPath.GenerateDubins(mf.pivotAxlePos, goal);

            //transfer point list to recPath class point style
            dubList.Clear();
            for (int i = 0; i < dubinsList.Count; i++)
            {
                CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
                dubList.Add(pt);
            }
        }
Ejemplo n.º 6
0
        private void GetDubinsPath(vec3 goal)
        {
            CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;
            CDubins dubPath = new CDubins();

            // current psition
            pivotAxlePosRP = mf.pivotAxlePos;

            //bump it forward
            vec3 pt2 = new vec3
            {
                easting  = pivotAxlePosRP.easting + (Math.Sin(pivotAxlePosRP.heading) * 3),
                northing = pivotAxlePosRP.northing + (Math.Cos(pivotAxlePosRP.heading) * 3),
                heading  = pivotAxlePosRP.heading
            };

            //get the dubins path vec3 point coordinates of turn
            shortestDubinsList.Clear();
            shuttleDubinsList.Clear();

            shortestDubinsList = dubPath.GenerateDubins(pt2, goal);

            //if Dubins returns 0 elements, there is an unavoidable blockage in the way.
            if (shortestDubinsList.Count > 0)
            {
                shortestDubinsList.Insert(0, mf.pivotAxlePos);

                //transfer point list to recPath class point style
                for (int i = 0; i < shortestDubinsList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 9.0, false);
                    shuttleDubinsList.Add(pt);
                }
                return;
            }
        }
Ejemplo n.º 7
0
        //build the points and path of youturn to be scaled and transformed
        public void BuildYouTurnListToRight(bool isTurnRight)
        {
            isYouTurnShapeDisplayed = true;

            CDubins dubYouTurnPath = new CDubins();

            CDubins.turningRadius = mf.vehicle.minTurningRadius;

            //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
            if (isTurnRight)
            {
                turnOffset = (widthMinusOverlap + toolOffset);
            }
            else
            {
                turnOffset = (widthMinusOverlap - toolOffset);
            }

            //if using dubins to calculate youturn
            if (isUsingDubinsTurn)
            {
                //if (lastTime == 3)
                //{
                //    lastTime = 5;
                //    rowSkipsWidth = 5;
                //}

                //else
                //{
                //    lastTime = 3;
                //    rowSkipsWidth = 3;
                //}

                double head = mf.ABLine.abHeading;
                //if its straight across it makes 2 loops instead so goal is a little lower then start
                if (!isABSameAsFixHeading)
                {
                    head += 3.14;
                }
                else
                {
                    head -= 0.01;
                }

                var start = new vec3(rEastYT, rNorthYT, head);
                var goal  = new vec3();

                //also adjust for rowskips
                head -= Math.PI;
                if (head < 0)
                {
                    head += glm.twoPI;
                }

                if (isTurnRight)
                {
                    goal.easting  = rEastYT - Math.Cos(-head) * turnOffset * rowSkipsWidth;
                    goal.northing = rNorthYT - Math.Sin(-head) * turnOffset * rowSkipsWidth;
                    goal.heading  = head;
                }
                else
                {
                    goal.easting  = rEastYT + Math.Cos(-head) * turnOffset * rowSkipsWidth;
                    goal.northing = rNorthYT + Math.Sin(-head) * turnOffset * rowSkipsWidth;
                    goal.heading  = head;
                }

                ytList = dubYouTurnPath.GenerateDubins(start, goal);
            }

            //or the patterns
            else
            {
                numShapePoints = youFileList.Count;
                vec3[] pt = new vec3[numShapePoints];

                //Now put the shape into an array since lists are immutable
                for (int i = 0; i < numShapePoints; i++)
                {
                    pt[i].easting  = youFileList[i].easting;
                    pt[i].northing = youFileList[i].northing;
                }

                //start of path on the origin. Mirror the shape if left turn
                if (!isTurnRight)
                {
                    for (int i = 0; i < pt.Length; i++)
                    {
                        pt[i].easting *= -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].easting  *= scale * rowSkipsWidth;
                    pt[i].northing *= scale * rowSkipsWidth;
                }

                //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].easting) - (Math.Sin(-abHeading) * pt[i].northing);
                        yr = (Math.Sin(-abHeading) * pt[i].easting) + (Math.Cos(-abHeading) * pt[i].northing);
                    }
                    else
                    {
                        xr = (Math.Cos(-abHeading + Math.PI) * pt[i].easting) - (Math.Sin(-abHeading + Math.PI) * pt[i].northing);
                        yr = (Math.Sin(-abHeading + Math.PI) * pt[i].easting) + (Math.Cos(-abHeading + Math.PI) * pt[i].northing);
                    }

                    pt[i].easting  = xr + rEastYT;
                    pt[i].northing = yr + rNorthYT;
                    pt[i].heading  = Math.Atan2(pt[i].northing, pt[i].easting);
                    ytList.Add(pt[i]);
                }
            }
        }
Ejemplo n.º 8
0
        public void GenerateHeadlandPath()
        {
            //first find out which side is inside the boundary
            //insideOutside = glm.PIBy2;
            //point.easting = mf.bnd.ptList[3].easting - (Math.Sin(insideOutside + mf.bnd.ptList[3].heading) * 2.0);
            //point.northing = mf.bnd.ptList[3].northing - (Math.Cos(insideOutside + mf.bnd.ptList[3].heading) * 2.0);
            //if (!mf.bnd.IsPointInsideBoundary(point)) insideOutside *= -1.0;

            //keep copy of original position/heading
            homePoint = mf.pivotAxlePos;

            //clear the main list
            genList.Clear();

            for (int passNumber = 1; passNumber <= numHeadlandPasses; passNumber++)
            {
                //determine the start point of first headland -
                accList.Clear();
                BuildOneHeadlandPath((mf.vehicle.toolWidth - mf.vehicle.toolOverlap) * ((double)passNumber - 0.4));
                startHeadlandPoint = accList[0];

                CDubins dubPath = new CDubins();
                CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.2;

                ////get the dubins path vec3 point coordinates of turn
                //dubList.Clear();
                //if (passNumber == 1) dubList = dubPath.GenerateDubins(mf.pivotAxlePos, startHeadlandPoint, mf.bnd);
                //else dubList = dubPath.GenerateDubins(endHeadlandPoint, startHeadlandPoint, mf.bnd);

                //transfer point list to genList class
                for (int i = 0; i < dubList.Count; i++)
                {
                    CGenPathPt pt = new CGenPathPt(dubList[i].easting, dubList[i].northing, dubList[i].heading);
                    if (i < 5)
                    {
                        pt.sequence = 1;
                    }
                    genList.Add(pt);
                }

                for (int i = 0; i < accList.Count; i++)
                {
                    CGenPathPt pt = new CGenPathPt(accList[i].easting, accList[i].northing, accList[i].heading);
                    if (i < 5)
                    {
                        pt.sequence = 2;
                    }
                    genList.Add(pt);
                }

                for (int i = 0; i < 5; i++)
                {
                    CGenPathPt pt = new CGenPathPt(accList[i].easting, accList[i].northing, accList[i].heading);
                    genList.Add(pt);
                }
                endHeadlandPoint = accList[5];
            }
            genListCount        = genList.Count;
            currentPositonIndex = 0;
            isEndOfTheGenLine   = false;
        }
Ejemplo n.º 9
0
        private void GetDubinsPath(vec3 goal)
        {
            //CDubins dubPath = new CDubins();

            ////
            //pivotPos = mf.pivotAxlePos;

            ////bump it forward
            //vec3 pt2 = new vec3
            //{
            //    easting = pivotPos.easting + (Math.Sin(pivotPos.heading) * 4),
            //    northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 4),
            //    heading = pivotPos.heading
            //};

            ////get the dubins path vec3 point coordinates of turn
            //dubinsList.Clear();
            //dubinsList = dubPath.GenerateDubins(pt2, goal);

            //dubinsList.Insert(0, mf.pivotAxlePos);

            ////transfer point list to recPath class point style
            //dubList.Clear();
            //for (int i = 0; i < dubinsList.Count; i++)
            //{
            //    CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 5.0, false);
            //    dubList.Add(pt);
            //}

            ////no longer needed
            //dubinsList?.Clear();

            CDubins.turningRadius = mf.vehicle.minTurningRadius * 1.0;
            CDubins dubPath = new CDubins();

            // current psition
            pivotPos = mf.pivotAxlePos;

            //bump it forward
            vec3 pt2 = new vec3
            {
                easting  = pivotPos.easting + (Math.Sin(pivotPos.heading) * 3),
                northing = pivotPos.northing + (Math.Cos(pivotPos.heading) * 3),
                heading  = pivotPos.heading
            };

            //get the dubins path vec3 point coordinates of turn
            dubinsList.Clear();
            dubList.Clear();

            dubinsList = dubPath.GenerateDubins(pt2, goal, mf.gf);

            //if Dubins returns 0 elements, there is an unavoidable blockage in the way.
            if (dubinsList.Count > 0)
            {
                dubinsList.Insert(0, mf.pivotAxlePos);

                //transfer point list to recPath class point style
                for (int i = 0; i < dubinsList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 9.0, false);
                    dubList.Add(pt);
                }
                return;
            }

            //find a path from start to goal - diagnostic, but also used later
            mazeList = mf.mazeGrid.SearchForPath(pt2, goal);

            //you can't get anywhere!
            if (mazeList == null)
            {
                return;
            }

            //start is navigateable - maybe
            int cnt = mazeList.Count;

            if (cnt > 0)
            {
                {
                    int turnRadius = (int)(3 * mf.vehicle.minTurningRadius);
                    if (cnt > 2 * turnRadius)
                    {
                        mazeList.RemoveRange(0, turnRadius);
                        cnt = mazeList.Count;
                        mazeList.RemoveRange(cnt - turnRadius, turnRadius);
                    }
                }

                dubinsList = dubPath.GenerateDubins(pt2, mazeList[0], mf.gf);
                if (dubinsList.Count > 0)
                {
                    for (int i = 0; i < dubinsList.Count; i++)
                    {
                        CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 10.0, false);
                        dubList.Add(pt);
                    }
                }
                else
                {
                    return; //unable to generate a dubins to the start
                }

                for (int i = 0; i < mazeList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(mazeList[i].easting, mazeList[i].northing, mazeList[i].heading, 15.0, false);
                    dubList.Add(pt);
                }

                dubinsList = dubPath.GenerateDubins(mazeList[mazeList.Count - 1], goal, mf.gf);

                for (int i = 0; i < dubinsList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(dubinsList[i].easting, dubinsList[i].northing, dubinsList[i].heading, 11.0, false);
                    dubList.Add(pt);
                }
                return;
            }
        }
Ejemplo n.º 10
0
        //build the points and path of youturn to be scaled and transformed
        public void BuildYouTurnListToRight(bool isTurnRight, bool isTurnButtonTriggered)
        {
            isYouTurnShapeDisplayed = true;

            CDubins dubYouTurnPath = new CDubins();

            CDubins.turningRadius = mf.vehicle.minTurningRadius;

            double delta;

            //point on AB line closest to pivot axle point from ABLine PurePursuit
            if (mf.ABLine.isABLineSet)
            {
                rEastYT              = mf.ABLine.rEastAB;
                rNorthYT             = mf.ABLine.rNorthAB;
                isABSameAsFixHeading = mf.ABLine.isABSameAsVehicleHeading;
                abHeading            = mf.ABLine.abHeading;
                delta = 1;
            }
            else
            {
                rEastYT              = mf.curve.rEastCu;
                rNorthYT             = mf.curve.rNorthCu;
                isABSameAsFixHeading = mf.curve.isSameWay;
                abHeading            = mf.curve.refHeading;
                delta = mf.curve.deltaOfRefAndAveHeadings;
            }

            //grab the vehicle widths and offsets
            double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
            double toolOffset        = mf.vehicle.toolOffset * 2.0;
            double turnOffset        = 0;

            //turning right
            if (isTurnRight)
            {
                turnOffset = (widthMinusOverlap + toolOffset);
            }
            else
            {
                turnOffset = (widthMinusOverlap - toolOffset);
            }

            //to compensate for AB Curve overlap
            turnOffset *= delta;

            double turnRadius = turnOffset / Math.Cos(boundaryAngleOffPerpendicular);

            //if using dubins to calculate youturn
            if (isUsingDubinsTurn)
            {
                double head = abHeading;

                //if its straight across it makes 2 loops instead so goal is a little lower then start
                if (!isABSameAsFixHeading)
                {
                    head += 3.14;
                }
                else
                {
                    head -= 0.01;
                }

                //move the start forward 2 meters
                rEastYT  += (Math.Sin(head) * 2);
                rNorthYT += (Math.Cos(head) * 2);

                var start = new vec3(rEastYT, rNorthYT, head);
                var goal  = new vec3();

                //also adjust for rowskips if Dew loops are set
                if (isDew2Set | isDew4Set)
                {
                    if (isDew2Set)
                    {
                        skips = dew2Skips[dew2Index];

                        //if at end of turn, restart sequence
                        dew2Index++;
                        if (dew2Index >= dew2Skips.Length)
                        {
                            dew2Index = 0;
                        }
                    }

                    if (isDew4Set)
                    {
                        skips = dew4Skips[dew4Index];

                        //if at end of turn, restart sequence
                        dew4Index++;
                        if (dew4Index >= dew4Skips.Length)
                        {
                            dew4Index = 0;
                        }
                    }
                }
                else
                {
                    skips = rowSkipsWidth;
                }

                turnRadius *= skips;
                turnOffset *= skips;

                //move the cross line calc to not include first turn
                goal.easting  = rEastYT + (Math.Sin(head) * ping);
                goal.northing = rNorthYT + (Math.Cos(head) * ping);

                //headland angle relative to vehicle heading to head along the boundary left or right
                double bndAngle = head - boundaryAngleOffPerpendicular + glm.PIBy2;

                //now we go the other way to turn round
                head -= Math.PI;
                if (head < 0)
                {
                    head += glm.twoPI;
                }

                if ((mf.vehicle.minTurningRadius * 1.98) < turnOffset)
                {
                    //are we right of boundary
                    if (boundaryAngleOffPerpendicular > 0)
                    {
                        if (isYouTurnRight) //which is actually right now
                        {
                            goal.easting  += (Math.Sin(bndAngle) * turnRadius);
                            goal.northing += (Math.Cos(bndAngle) * turnRadius);

                            double dis = (mf.vehicle.minTurningRadius / Math.Tan(tangencyAngle)); //long
                            goal.easting  += (Math.Sin(head) * dis);
                            goal.northing += (Math.Cos(head) * dis);
                        }
                        else //going left
                        {
                            goal.easting  -= (Math.Sin(bndAngle) * turnRadius);
                            goal.northing -= (Math.Cos(bndAngle) * turnRadius);

                            double dis = (mf.vehicle.minTurningRadius * Math.Tan(tangencyAngle)); //short
                            goal.easting  += (Math.Sin(head) * dis);
                            goal.northing += (Math.Cos(head) * dis);
                        }
                    }
                    else // going left of boundary
                    {
                        if (isYouTurnRight) //pointing to right
                        {
                            goal.easting  += (Math.Sin(bndAngle) * turnRadius);
                            goal.northing += (Math.Cos(bndAngle) * turnRadius);

                            double dis = (mf.vehicle.minTurningRadius * Math.Tan(tangencyAngle)); //short
                            goal.easting  += (Math.Sin(head) * dis);
                            goal.northing += (Math.Cos(head) * dis);
                        }
                        else
                        {
                            goal.easting  -= (Math.Sin(bndAngle) * turnRadius);
                            goal.northing -= (Math.Cos(bndAngle) * turnRadius);

                            double dis = (mf.vehicle.minTurningRadius / Math.Tan(tangencyAngle)); //long
                            goal.easting  += (Math.Sin(head) * dis);
                            goal.northing += (Math.Cos(head) * dis);
                        }
                    }
                }
                else
                {
                    if (isTurnRight)
                    {
                        goal.easting  = rEastYT - (Math.Cos(-head) * turnOffset);
                        goal.northing = rNorthYT - (Math.Sin(-head) * turnOffset);
                    }
                    else
                    {
                        goal.easting  = rEastYT + (Math.Cos(-head) * turnOffset);
                        goal.northing = rNorthYT + (Math.Sin(-head) * turnOffset);
                    }
                }

                goal.heading = head;

                if (isTurnButtonTriggered)
                {
                    if (isTurnRight)
                    {
                        goal.easting  = rEastYT - (Math.Cos(-head) * turnOffset);
                        goal.northing = rNorthYT - (Math.Sin(-head) * turnOffset);
                    }
                    else
                    {
                        goal.easting  = rEastYT + (Math.Cos(-head) * turnOffset);
                        goal.northing = rNorthYT + (Math.Sin(-head) * turnOffset);
                    }
                }

                //generate the turn points
                ytList = dubYouTurnPath.GenerateDubins(start, goal);
            }

            //or the patterns
            else
            {
                numShapePoints = youFileList.Count;
                vec3[] pt = new vec3[numShapePoints];

                //Now put the shape into an array since lists are immutable
                for (int i = 0; i < numShapePoints; i++)
                {
                    pt[i].easting  = youFileList[i].easting;
                    pt[i].northing = youFileList[i].northing;
                }

                //start of path on the origin. Mirror the shape if left turn
                if (!isTurnRight)
                {
                    for (int i = 0; i < pt.Length; i++)
                    {
                        pt[i].easting *= -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].easting  *= scale * rowSkipsWidth;
                    pt[i].northing *= scale * rowSkipsWidth;
                }

                //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].easting) - (Math.Sin(-abHeading) * pt[i].northing);
                        yr = (Math.Sin(-abHeading) * pt[i].easting) + (Math.Cos(-abHeading) * pt[i].northing);
                    }
                    else
                    {
                        xr = (Math.Cos(-abHeading + Math.PI) * pt[i].easting) - (Math.Sin(-abHeading + Math.PI) * pt[i].northing);
                        yr = (Math.Sin(-abHeading + Math.PI) * pt[i].easting) + (Math.Cos(-abHeading + Math.PI) * pt[i].northing);
                    }

                    pt[i].easting  = xr + rEastYT;
                    pt[i].northing = yr + rNorthYT;
                    pt[i].heading  = Math.Atan2(pt[i].northing, pt[i].easting);
                    ytList.Add(pt[i]);
                }
            }
        }
Ejemplo n.º 11
0
        private void GetDubinsPath(vec3 goal)
        {
            CDubins.turningRadius = mf.vehicle.minTurningRadius * 2.0;
            CDubins dubPath = new CDubins();

            // current psition
            pivotAxlePosRP = mf.pivotAxlePos;

            //bump it forward
            vec3 pt2 = new vec3
            {
                easting  = pivotAxlePosRP.easting + (Math.Sin(pivotAxlePosRP.heading) * 3),
                northing = pivotAxlePosRP.northing + (Math.Cos(pivotAxlePosRP.heading) * 3),
                heading  = pivotAxlePosRP.heading
            };

            //get the dubins path vec3 point coordinates of turn
            shortestDubinsList.Clear();
            shuttleDubinsList.Clear();

            shortestDubinsList = dubPath.GenerateDubins(pt2, goal, mf.gf);

            //if Dubins returns 0 elements, there is an unavoidable blockage in the way.
            if (shortestDubinsList.Count > 0)
            {
                shortestDubinsList.Insert(0, mf.pivotAxlePos);

                //transfer point list to recPath class point style
                for (int i = 0; i < shortestDubinsList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 9.0, false);
                    shuttleDubinsList.Add(pt);
                }
                return;
            }
            //else
            //{
            //    shuttleDubinsList?.Clear();
            //    return;
            //}

            //find a path from start to goal - diagnostic, but also used later
            mazeList = mf.mazeGrid.SearchForPath(pt2, goal);

            //you can't get anywhere!
            if (mazeList == null)
            {
                return;
            }

            //start is navigateable - maybe
            if (mazeList.Count > 0)
            {
                shortestDubinsList = dubPath.GenerateDubins(pt2, mazeList[0], mf.gf);
                for (int i = 0; i < shortestDubinsList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 10.0, false);
                    shuttleDubinsList.Add(pt);
                }
                for (int i = 0; i < mazeList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(mazeList[i].easting, mazeList[i].northing, mazeList[i].heading, 15.0, false);
                    shuttleDubinsList.Add(pt);
                }

                shortestDubinsList = dubPath.GenerateDubins(mazeList[mazeList.Count - 1], goal, mf.gf);

                for (int i = 0; i < shortestDubinsList.Count; i++)
                {
                    CRecPathPt pt = new CRecPathPt(shortestDubinsList[i].easting, shortestDubinsList[i].northing, shortestDubinsList[i].heading, 11.0, false);
                    shuttleDubinsList.Add(pt);
                }
                return;
            }
            else
            {
            }
        }
Ejemplo n.º 12
0
        //build the points and path of youturn to be scaled and transformed
        public void BuildYouTurnListToRight(bool isTurnRight)
        {
            isYouTurnShapeDisplayed = true;

            CDubins dubYouTurnPath = new CDubins();

            CDubins.turningRadius = mf.vehicle.minTurningRadius;

            double delta;

            //point on AB line closest to pivot axle point from ABLine PurePursuit
            if (mf.ABLine.isABLineSet)
            {
                rEastYT              = mf.ABLine.rEastAB;
                rNorthYT             = mf.ABLine.rNorthAB;
                isABSameAsFixHeading = mf.ABLine.isABSameAsFixHeading;
                abHeading            = mf.ABLine.abHeading;
                delta = 1;
            }
            else
            {
                rEastYT              = mf.curve.rEastCu;
                rNorthYT             = mf.curve.rNorthCu;
                isABSameAsFixHeading = mf.curve.isSameWay;
                abHeading            = mf.curve.refHeading;
                delta = mf.curve.deltaOfRefAndAveHeadings;
            }

            //grab the vehicle widths and offsets
            double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
            double toolOffset        = mf.vehicle.toolOffset * 2.0;
            double turnOffset        = 0;

            //turning right
            if (isTurnRight)
            {
                turnOffset = (widthMinusOverlap + toolOffset);
            }
            else
            {
                turnOffset = (widthMinusOverlap - toolOffset);
            }

            //to compensate for AB Curve overlap
            turnOffset *= delta;

            //if using dubins to calculate youturn
            if (isUsingDubinsTurn)
            {
                int    skips = 0;
                double head  = abHeading;

                //if its straight across it makes 2 loops instead so goal is a little lower then start
                if (!isABSameAsFixHeading)
                {
                    head += 3.14;
                }
                else
                {
                    head -= 0.01;
                }

                var start = new vec3(rEastYT, rNorthYT, head);
                var goal  = new vec3();

                head -= Math.PI;
                if (head < 0)
                {
                    head += glm.twoPI;
                }

                //also adjust for rowskips if Dew loops are set
                if (isDew2Set | isDew4Set)
                {
                    if (isDew2Set)
                    {
                        skips = dew2Skips[dew2Index];

                        //if at end of turn, restart sequence
                        dew2Index++;
                        if (dew2Index >= dew2Skips.Length)
                        {
                            dew2Index = 0;
                        }
                    }

                    if (isDew4Set)
                    {
                        skips = dew4Skips[dew4Index];

                        //if at end of turn, restart sequence
                        dew4Index++;
                        if (dew4Index >= dew4Skips.Length)
                        {
                            dew4Index = 0;
                        }
                    }
                }
                else
                {
                    skips = rowSkipsWidth;
                }

                //calculate the turn
                if (isTurnRight)
                {
                    goal.easting  = rEastYT - (Math.Cos(-head) * turnOffset * skips);
                    goal.northing = rNorthYT - (Math.Sin(-head) * turnOffset * skips);
                    goal.heading  = head;
                }
                else
                {
                    goal.easting  = rEastYT + (Math.Cos(-head) * turnOffset * skips);
                    goal.northing = rNorthYT + (Math.Sin(-head) * turnOffset * skips);
                    goal.heading  = head;
                }

                //generate the turn points
                ytList = dubYouTurnPath.GenerateDubins(start, goal);
            }

            //or the patterns
            else
            {
                numShapePoints = youFileList.Count;
                vec3[] pt = new vec3[numShapePoints];

                //Now put the shape into an array since lists are immutable
                for (int i = 0; i < numShapePoints; i++)
                {
                    pt[i].easting  = youFileList[i].easting;
                    pt[i].northing = youFileList[i].northing;
                }

                //start of path on the origin. Mirror the shape if left turn
                if (!isTurnRight)
                {
                    for (int i = 0; i < pt.Length; i++)
                    {
                        pt[i].easting *= -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].easting  *= scale * rowSkipsWidth;
                    pt[i].northing *= scale * rowSkipsWidth;
                }

                //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].easting) - (Math.Sin(-abHeading) * pt[i].northing);
                        yr = (Math.Sin(-abHeading) * pt[i].easting) + (Math.Cos(-abHeading) * pt[i].northing);
                    }
                    else
                    {
                        xr = (Math.Cos(-abHeading + Math.PI) * pt[i].easting) - (Math.Sin(-abHeading + Math.PI) * pt[i].northing);
                        yr = (Math.Sin(-abHeading + Math.PI) * pt[i].easting) + (Math.Cos(-abHeading + Math.PI) * pt[i].northing);
                    }

                    pt[i].easting  = xr + rEastYT;
                    pt[i].northing = yr + rNorthYT;
                    pt[i].heading  = Math.Atan2(pt[i].northing, pt[i].easting);
                    ytList.Add(pt[i]);
                }
            }
        }