The Combine Forum banner
Cancel
Create thread New Forums

DIY AutoSteer with AgOpenGPS

Jump to Latest
281 - 300 of 2,032 Posts
#281 ·
Howdy all, just wondering if anyone has headland turns automated? Looking at moving the tractor to the field with ardupilot then letting agopengps take over for each field run? Not sure if it will work or not but I want to get the whole lot automated
 
Discussion starter · #283 ·
Anyone adventurous enough to guess what's going on here?

Code: 
        //build the points and path 
        public void BuildAutoList(bool isTurnRight)
        {
            //point on AB line closest to pivot axle point from ABLine PurePursuit
            rEastAT = mf.ABLine.rEastAB;
            rNorthAT = mf.ABLine.rNorthAB;
            isABSameAsFixHeading = mf.ABLine.isABSameAsFixHeading;

            //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)
            {
                turnOffset = ((widthMinusOverlap + toolOffset) * 0.5);
                turnHeading = abHeading + glm.PIBy2;
            }

            //turning left same way as AB line
            if (!isTurnRight && isABSameAsFixHeading)
            {
                turnOffset = ((widthMinusOverlap - toolOffset) * 0.5);
                turnHeading = abHeading - glm.PIBy2;
            }

            //turning right opposite way as AB line
            if (isTurnRight && !isABSameAsFixHeading)
            {
                turnOffset = ((widthMinusOverlap + toolOffset) * 0.5);
                turnHeading = abHeading - glm.PIBy2;
                //maybe flip 180 degrees
            }

            //turning left opposite way as AB line
            if (!isTurnRight && !isABSameAsFixHeading)
            {
                turnOffset = ((widthMinusOverlap - toolOffset) * 0.5);
                turnHeading = abHeading + glm.PIBy2;
            }

            //clear it out
            atList.Clear();

            if (!isABSameAsFixHeading) abHeading += Math.PI;

            //grab a copy from main
            pivotAxlePosAT = mf.pivotAxlePos;
            vec4 point = new vec4(rEastAT, abHeading, rNorthAT, 0);
            atList.Add(point);
            int i;

            for (i = 1; i < 8; i++)
            {
                point.x = Math.Sin(abHeading) * i + rEastAT;
                point.y = abHeading;
                point.z = Math.Cos(abHeading) * i + rNorthAT;
                point.k = 0;
                atList.Add(point);
            }          
 
            //determine the center of the turn around arc
            double temp = Math.Sin(abHeading) * 8 + rEastAT;
            turnRadiusPt.easting = Math.Sin(turnHeading) * turnOffset + temp;
            temp = Math.Cos(abHeading) * 8 + rNorthAT;
            turnRadiusPt.northing = Math.Cos(turnHeading) * turnOffset + temp;
            
            int numSegments = 20;
            double theta;

            if (isTurnRight)
            {
                for (i = 0; i <= numSegments; i++)
                {
                    theta = (glm.twoPI) * (double)(i) / ((double)(numSegments)*2.0); 
                    point.x = turnOffset * Math.Sin(theta + turnHeading + Math.PI) + turnRadiusPt.easting;
                    point.z = turnOffset * Math.Cos(theta + turnHeading + Math.PI) + turnRadiusPt.northing;
                    point.y = Math.Atan2(point.z, point.x);
                    atList.Add(point);            
                }
            }

            else
            {
                for ( i = numSegments; i >= 0; i--)
                {
                    theta = (glm.twoPI) * (double)(i) / ((double)(numSegments) * 2.0);
                    point.x = turnOffset * Math.Sin(theta + turnHeading) + turnRadiusPt.easting;
                    point.z = turnOffset * Math.Cos(theta + turnHeading) + turnRadiusPt.northing;
                    point.y = Math.Atan2(point.z, point.x);
                    atList.Add(point);            
                }
            }

            for (i = 7; i >= 0; i--)
            {
                temp = Math.Sin(abHeading) * i + rEastAT;
                point.x = Math.Sin(turnHeading) * turnOffset*2 + temp;
                temp = Math.Cos(abHeading) * i + rNorthAT;
                point.z = Math.Cos(turnHeading) * turnOffset*2 + temp;
                point.y = abHeading - Math.PI;
                point.k = 0;
                atList.Add(point);
            }
               
            if (!isABSameAsFixHeading) abHeading += Math.PI; 
        }

        //determine distance from autoTurn guidance line
        public void DistanceFromAutoTurnLine()
        {
            //grab a copy from main
            pivotAxlePosAT = mf.pivotAxlePos;
            double minDistA = 1000000, minDistB = 1000000;
            int ptCount = atList.Count;

            if (ptCount > 0)
            {
                //find the closest 2 points to current fix
                for (int t = 0; t < ptCount; t++)
                {
                    double dist = ((pivotAxlePosAT.easting - atList[t].x) * (pivotAxlePosAT.easting - atList[t].x))
                                    + ((pivotAxlePosAT.northing - atList[t].z) * (pivotAxlePosAT.northing - atList[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 = atList[B].x - atList[A].x;
                double dz = atList[B].z - atList[A].z;
                if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon) return;

                //abHeading = Math.Atan2(dz, dx);
                abHeading = atList[A].y;

                //how far from current AB Line is fix
                distanceFromCurrentLine = ((dz * mf.pn.easting) - (dx * mf.pn.northing) + (atList[B].x
                            * atList[A].z) - (atList[B].z * atList[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)
                {
                    isAutoTurnOn = false;
                    atList.Clear();
                    return;
                }

                // ** Pure pursuit ** - calc point on ABLine closest to current position      
                double U = (((pivotAxlePosAT.easting - atList[A].x) * (dx))
                            + ((pivotAxlePosAT.northing - atList[A].z) * (dz)))
                            / ((dx * dx) + (dz * dz));

                rEastAT = atList[A].x + (U * (dx));
                rNorthAT = atList[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 4.0 meters look ahead
                if (goalPointDistance < 3.0) goalPointDistance = 3.0;

                // used for calculating the length squared of next segment.
                double tempDist = 0.0;

                isABSameAsFixHeading = true;
                distSoFar = mf.pn.Distance(atList[B].z, atList[B].x, rNorthAT, rEastAT);

                //Is this segment long enough to contain the full lookahead distance?
                if (distSoFar > goalPointDistance)
                {
                    //treat current segment like an AB Line
                    goalPointAT.easting = rEastAT + (Math.Sin(atList[A].y) * goalPointDistance);
                    goalPointAT.northing = rNorthAT + (Math.Cos(atList[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(atList[B].z, atList[B].x, atList[A].z, atList[A].x);
                        if ((tempDist + distSoFar) > goalPointDistance) break; //will we go too far?
                        distSoFar += tempDist;
                    }

                    double t = (goalPointDistance - distSoFar); // the remainder to yet travel
                    t /= tempDist;
                    goalPointAT.easting = (((1 - t) * atList[A].x) + (t * atList[B].x));
                    goalPointAT.northing = (((1 - t) * atList[A].z) + (t * atList[B].z));
                }

                //calc "D" the distance from pivot axle to lookahead point
                double goalPointDistanceSquared = mf.pn.DistanceSquared(goalPointAT.northing, goalPointAT.easting, pivotAxlePosAT.northing, pivotAxlePosAT.easting);

                //calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
                double localHeading = glm.twoPI - mf.fixHeading;
                ppRadiusAT = goalPointDistanceSquared / (2 * (((goalPointAT.easting - pivotAxlePosAT.easting) * Math.Cos(localHeading)) + ((goalPointAT.northing - pivotAxlePosAT.northing) * Math.Sin(localHeading))));

                steerAngleAT = glm.toDegrees(Math.Atan(2 * (((goalPointAT.easting - pivotAxlePosAT.easting) * Math.Cos(localHeading))
                    + ((goalPointAT.northing - pivotAxlePosAT.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));

                if (steerAngleAT < -mf.vehicle.maxSteerAngle) steerAngleAT = -mf.vehicle.maxSteerAngle;
                if (steerAngleAT > mf.vehicle.maxSteerAngle) steerAngleAT = mf.vehicle.maxSteerAngle;

                if (ppRadiusAT < -500) ppRadiusAT = -500;
                if (ppRadiusAT > 500) ppRadiusAT = 500;

                radiusPointAT.easting = pivotAxlePosAT.easting + (ppRadiusAT * Math.Cos(localHeading));
                radiusPointAT.northing = pivotAxlePosAT.northing + (ppRadiusAT * Math.Sin(localHeading));

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

                //clamp the steering angle to not exceed safe angular velocity
                if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
                {
                    steerAngleAT = glm.toDegrees(steerAngleAT > 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)(steerAngleAT * 10);
            }
            else 
            {
                isAutoTurnOn = false;
                if (atList.Count > 0) atList.Clear();
                return;
        }
 
#286 ·
Turning is really tricky stuff. For example with my air drill and pull between cart, the turn is no simple curve. The goal is to get the drill turning such that the packer wheels stop on the inside side as I'm turning, then go slightly backwards, and then forward as I straighten out. Any tighter than that and the drill pulls too much to the inside as it straightens out (overlapping my last pass), any less, and there will be a gap between the last pass and the current pass for a few meters. I think an algorithm could figure that out, but it would take a lot of tricky modeling!
 
#287 · (Edited)
BrianTee said:
Anyone adventurous enough to guess what's going on here?
Click to expand...
:sFun_crazy4:
Headland turn oh Yeah!!

Is it (easy) possible to automatically scale the icons/buttons?--> When i scale down the buttons a bit, the screen fits perfectly in a 800x600 screen size:

Image


I now use the software on a Lattepanda, which will be connected via an RCA connector to the Intelliview 4 display in the new Holland. Advantage: No additional display, disadvantage limited resolution ... the icons are now outside the screen.
 
#288 · (Edited)
@Mhoek
am looking for a suitable steering angle sensor, someone an idea where I could buy this in Europe?
Is it correct that it is not possible with the current version of the software to store and load A-B lines? (and to shift them parallel?)
(or did I not look good enough??)
Click to expand...
i did use this:

http://www.robotshop.com/eu/en/cytron-30a-5-30v-single-brushed-dc-motor-driver.html

Model SLPC 300 - Sensormarket GmbH
other something else search for Linear Potentiometer

or you can use a hall sensor from every car like:

AUDI > A3 > A4 > A6 > A8 > TT Niveausensor Frontlicht 4B0907503A | eBay

most of the have 6 pin but you need only 3 of them..
 
#289 · (Edited)
You will probably want to do that here:

//convert position to steer angle. 4 counts per degree of steer pot position in my case
// ***** make sure that negative steer angle makes a left turn and positive value is a right turn *****
// remove or add the minus for steerSensorCounts to do that.
steerAngleActual = (float)(steeringPosition)/-steerSensorCounts;

Your steerAngleAcual is your actual steer angle - good variable name!. So now just do a quick check if it is greater then + or - 25 degrees, and turn on off 4wd like so:

if (steerAngleActual > abs(25) {
Turn off the output pin for 4 wd }

else {
Turn on the 4wd pin }

BTW Andreas, is the BTS7960 driver board working well?
Click to expand...
ok thank you!

the bts7960 board is working - i think. i did only do a short test in my office.

i did get a Cytron 13Amp DC Motor Driver so am 1:1 with your work!

i hope i can get all on my tractor next week.

what do we need for working with imu? i have a MPU9250 ...
 
#290 ·
torriem said:
Turning is really tricky stuff. For example with my air drill and pull between cart, the turn is no simple curve. The goal is to get the drill turning such that the packer wheels stop on the inside side as I'm turning, then go slightly backwards, and then forward as I straighten out. Any tighter than that and the drill pulls too much to the inside as it straightens out (overlapping my last pass), any less, and there will be a gap between the last pass and the current pass for a few meters. I think an algorithm could figure that out, but it would take a lot of tricky modeling!
Click to expand...
We usually do 2 headland passes which allows for a wider turn so that it's not quite so tricky. I wonder if there's a symmetrical turn/path the tractor can take that would achieve the same thing (turn B in sketch).

I'm also curious now what it takes to record a turn and have AoG mimic that?
 

Attachments

Discussion starter · #291 · (Edited)
m_elias said:
I'm also curious now what it takes to record a turn and have AoG mimic that?
Click to expand...
A lot of Advil on my part.

The hardest part has been integrating the code into the existing AB Line guidance. The pattern was chosen because its "easy" to generate, although the whole thing is trig he11. The traditional XY trig and the way you learned in school has 0 Pi pointing out to the right. Then the radians increase as you go counter clockwise. GPS has the zero PI pointing north, so x is y, sin is cos and all vice versa - and the added fun bonus, radians increase the opposite way.

Also, you can turn left and right, but you can also go the other way left right so its backwards and upside down.

The path itself is generated, so that is on its own, and all the rest of code just uses that as a contour path for guidance. I just copy and pasted the contour guidance code into the autoTurn class and then wrote a path generator. Its basically autonomous path planning.

The AB Line guidance just keeps thinking its in control and at the end when the values are used, it gets its values replaced by the autoTurn if enabled. As soon as the autoturn finishes, its shuts itself off and the AB Line now doesn't have its values changed and works as normal. If you abort the turn, the AB Line guidance just kicks right in again - because it never stopped. Time spent thinking while harvesting has its benefits.

Basically the autoTurn is a virus to the AB Line guidance.

But as far as a pattern, we can use anything we want. Recording it, eek, that might take some thinking. We do 4 headlands, lots of room. I'm thinking a keyhole type pattern would be good as a first try to improve. The pattern i'm using is just an arc - perfect for a sprayer. But again, all we're doing is making a contour path and then follow it, it can be a DNA helix if you want. :1:

What i need to figure out is how to make a pattern with fixed coordinates that can then be scaled and rotated into place.
 
#292 ·
m_elias said:
We usually do 2 headland passes which allows for a wider turn so that it's not quite so tricky. I wonder if there's a symmetrical turn/path the tractor can take that would achieve the same thing (turn B in sketch).

I'm also curious now what it takes to record a turn and have AoG mimic that?
Click to expand...
We do two headland passes too. But even still, if you don't get the turn exactly right, you'll have a 6" miss for about 20-30 feet once you get off the headland passes. For kicks I should record the GPS track of a good turn and plot it some time. Honestly for 99% of the time, just recording a good turn and using that for guidance on subsequent turns would be good enough. Just press a button to start the turn and the computer would start replaying your turn. No algorithms necessary!

As far as Brian's headaches go, actually even contour guidance with an air drill becomes quite difficult. Essentially we want to match the drill's path, not the tractor's path. But I suspect it works just as well to ignore all that. That's what the commercial systems do.
 
Discussion starter · #293 ·
Not sure why i didn't think of this the first time. Create the shape from coordinates. Translate left or right depending on turn direction. Scale to match the tool width. Rotate based on AB Line heading and direction of travel. Easy Peasy

So now you can make your own custom plot of your headland turn. This code makes the attached headland path. So actually now you could make a DNA helix headland turn, if you wanted to. Or the Torriem Turn!

Code: 
           //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));
            }

            vec4[] pt = new vec4[37];

            pt[0].x = -5.0;     pt[0].z = 0.0;
            pt[1].x = -5.0;     pt[1].z = 1.0;
            pt[2].x = -5.1;     pt[2].z = 2.0;
            pt[3].x = -5.35;    pt[3].z = 3.0; 
            pt[4].x = -5.7;     pt[4].z = 4.0; 
            pt[5].x = -6.2;     pt[5].z = 5.0; 
            pt[6].x = -6.6;      pt[6].z = 6.0;
            pt[7].x = -6.8;      pt[7].z = 7.0;
            pt[8].x = -6.9;      pt[8].z = 8.0;
            pt[9].x = -6.8;      pt[9].z = 9.0;
            pt[10].x = -6.6;      pt[10].z = 10.0;
            pt[11].x = -6.3;      pt[11].z = 10.9;
            pt[12].x = -5.7;      pt[12].z = 11.8;
            pt[13].x = -5.0;      pt[13].z = 12.7;
            pt[14].x = -4.0;      pt[14].z = 13.6;
            pt[15].x = -3.0;      pt[15].z = 14.35;
            pt[16].x = -2.0;      pt[16].z = 14.7;
            pt[17].x = -1.0;      pt[17].z = 14.9;
            pt[18].x = 0.0;       pt[18].z = 15.0;
            pt[19].x = 1;         pt[19].z = 14.9;
            pt[20].x = 2;         pt[20].z = 14.7;
            pt[21].x = 3;         pt[21].z = 14.35;
            pt[22].x = 4;         pt[22].z = 13.6;
            pt[23].x = 5;         pt[23].z = 12.7;
            pt[24].x = 5.7;       pt[24].z = 11.8;
            pt[25].x = 6.3;       pt[25].z = 10.9;
            pt[26].x = 6.6;       pt[26].z = 10;
            pt[27].x = 6.8;       pt[27].z = 9.0;
            pt[28].x = 6.9;       pt[28].z = 8.0;
            pt[29].x = 6.8;       pt[29].z = 7.0;
            pt[30].x = 6.6;       pt[30].z = 6.0;
            pt[31].x = 6.2;       pt[31].z = 5.0;
            pt[32].x = 5.7;       pt[32].z = 4.0;
            pt[33].x = 5.35;      pt[33].z = 3.0;
            pt[34].x = 5.1;       pt[34].z = 2.0;
            pt[35].x = 5.0;       pt[35].z = 1.0;
            pt[36].x = 5.0;       pt[36].z = 0.0;
 
            //start of path on the origin. Easy for rotation
            if (isTurnRight)
            {
                for (int i = 0; i < pt.Length; i++) pt[i].x += 5;
            }

            else //left turn
            {
                for (int i = 0; i < pt.Length; i++) pt[i].x -= 5;
                Array.Reverse(pt);
            }

            //scaling
            double scale = turnOffset / 10.0;
            for (int i = 0; i < pt.Length; i++)
            {
                pt[i].x *= scale; pt[i].z *= scale;
            }
 
            //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+rEastAT;;
                pt[i].z = yr+rNorthAT;
                pt[i].k = 0;
                pt[i].y = Math.Atan2(pt[i].z, pt[i].x);
                atList.Add(pt[i]);
            }
        }
 

Attachments

#294 ·
Andreas Ortner said:
i did use this:
..
Click to expand...
Andreas,

Yesterday i posted a reply with the items i bought.. but because of the forum rules a new member is not allowed to use hyperlinks or write links as text.. :53:
so my reply has been deleted..

In short: I ordered an H-bridge with a BTS7960B driver and a Level sensor type : RQH100030 on aliexpress.
I wil keep you informed of my findings... when the items arrive
 
Discussion starter · #300 ·
Andreas Ortner said:
O M G !!!!

Brian!!! WOW!!

What IMU do we need?
Click to expand...
I have been using the BNO055 from Bosch. BNO055 9 DOF Absolute Orientation IMU Fusion Breakout Board - RobotShop

But its a fussy thing, and a bit buggy. The tilt and roll start to wander if moving - known bug, it needs calibration, and it does its own calibration when it shouldn't. But, i have found the heading to work pretty well.

Looking for an option that does tilt and heading all in one would be nice. But so far have not found one that doesn't cost 1000$ or more.

For tilt i have been using https://ca.mouser.com/ProductDetail/TE-Connectivity/G-NSDOG2-001/?qs=aXGKoampmnkGHg5WKH0mvw==

Its an industrial inclinometer that is rock solid - outputs at 100Hz, and is analog out. Requires no calibration and when it is level it outputs 2.5v exactly.

Right now i am using the BNO055 on the relay port on its own Arduino, but no reason the autosteer module couldn't do Tilt, heading, autosteer. But it would require a rewrite as the heading and tilt information are required right after getting the nmea sentence. I wrote a Kalman filter for tilt, works quite well.

As usual, more ideas then time, but i think it is really coming together.

So the search is on for a good imu / AHRS that does heading and tilt that works and isn't crazy expensive.
 
281 - 300 of 2,032 Posts
About this Discussion
2K Replies
123 Participants
Last post:
penny199666
The Combine Forum
posts
916K
members
61K
Since
2005
A forum community dedicated to all combine owners and enthusiasts. Come join the discussion about specifications, accessories, troubleshooting, maintenance, and more!
Full Forum Listing
Explore Our Forums
John Deere Tractors Case IH Planting & Tillage Crop Production