Yes, 2.2 mph is 3.2 ft per sec. It is the standard start up speed. 3.5 kph
The program really is set up for farm type of equipment, as in like 10 foot 3m minimum. Not sure of all the dynamics caused by really narrow settings
Agreed, that’s why I said I don’t think this is a bug. However, we have a small over pasture seeder and smaller field mowers that are less than this range and thought others might have run into the same problem. Never expected it to get into the main code, but just sharing a work around if someone needed. it. Thanks.
1 Like
I think it is important part of AOG to work with small eq like mowers. Mostly it comes down to making sure the boundary points are close enough - but not too close when using a 40m tool on a 100 ha field. this winter i will get some time to look at that.
1 Like
As you get into it, it has something to do with how sharp the next point on the turn line is from where you are now. It gets into the radian/heading error. In other words if the next point is too far (in angle) to the right or left and you are in those special quadrants where atan2 changes sign. It just doesn’t know what to do. You can run a 1 meter tool as long as you make very wide swings. There has to be enough yellow dots so the next one is less than about 40 degrees from current heading. Much more than that, it gets jumpy. Well after that it just gives up looking and gets hung in the u-turn sequence. I ran into this in drawing the ABRecordedPath. It really did not matter how wide was the tool. It mattered what was the angle of the next dot/line. If it is too sharp, random results. Especially in those special ATan2 quadrants. South-southwest to a north-northwest was a particular hard turn to get the simulator to calculate.
1 Like
You can make it do it with a wide tool as well. Attached is a 20 ft boom. It will turn left but will not turn right. There are no skips set. But it will turn left like it is supposed to and turn 90 degrees and pick back up a few lines later. Tractor and steer settings are in the image below. The heading is 0/180. There is a ratio between turn radius and wheelbase that sets it off. 100/120 is one of them 120/160 is another. I’m not sure that a tractor is built with these settings.
With same settings but 1 mph you can see that steering arrow briefly goes to left before it leaves u-turn (not able to snap back to desired track)
Could that be gap in calculation or mathematically opposite sign in calculation just after 90 degree in right U-turn.
Tried in older version from beginning of oktober, and there it would snap (small snap 10 big snap 50) back to desired U-turn when in automatic U-turn mode and turning right, but not when clicking manual U-turn right.(but still shows the brief opposite turn just after 90 degree)
There is a ratio between turn radius and wheelbase that sets it off. 100/120 is one of them 120/160 is another.
Just tried 100 for wheelbase and 133 for turn radius. The sin of steer angle is equal to the wheelbase over radius which gives around 50 degrees, a pretty sharp turn angle. But, the code in Stanley limits the max steer angle based on max steer angle. so that needs to be set at least 60 or 70 to compensate for delays of second order integration during the turn as well - it’s not instantaneous even in the sim. Because remember, you are turning away from the guidance line as you get closer, not steering towards the line. It is extremely complicated to do it any other way then second order int.
Again, AOG sets limits in code to prevent over steer and crazy steer in a real farm tractor setting which certainly limits very small wheelbase/radius combinations. All the code would have to be changed to go from a real farm size tractor to a small garden tractor. A realistic max is 50 degrees, so you can calculate your realistic wheelbase/radius and sin of steer angle based on that.
Picture at 100 wb, 133 radius.
I agree with this as well, but being able to turn to the left and not right is not a safety feature. Since the steer angle to the right has already been set to the maximum right for the first two to three segments of the u-turn line, then just hold it there until the end of the problem. I’m not suggesting re-writing the code, I’m simply offering a work around for those “garden” tractors that can turn sharper. That’s the beauty of open source. Individuals can adjust to meet their needs.
The proper fix is to enable the entire steering algorithm to work as smoothly with any wheelbase/turn radius within reason like it does with the larger ones. I guess what i am suggesting is yes, rewriting the code completely so it does work. Many tractors are set to 50 or even 55 degrees turn, so forcing a turn above 40 may cause problems in other areas - unintended.
Are you willing to rewrite all the steering code to work with small eq? Would be greatly appreciated. Perhaps there needs to be 2 alogorithms, one for normal, one for small.
The real work would be in figuring out the why.
I agree with KentStuff, and maybe it is only algorithm to right that has to be changed. It can turn left very steady (same direction and almost max of steering all the way) with these settings: 10 ft implement, radius 80, wheelbase 90 and max steering at 55 degree, and to the right it does find it´s way back to correct track by itself after those 2 or 3 times turning shortly left.
Or just use the -4000 +4000 fix that KentStuff suggested at the top.
Else change to Pure P, it can handle even smaller tractors below 2 mph.
I believe I have found the why. In the attached picture, the top shows a right turn, the bottom shows a left turn. Same tool settings, even though ridiculous settings, it is just to show the problem. I changed the U-Turn drawing code to draw a line from pivot to B point.
Turning Right:
As soon as the line jumps to quadrant IV (over your right shoulder), ATAN2 believes it is in quadrant II (in front of you to the left.) When the subtraction takes place. It is a double negative which adds and sends it left or straight. You can see it in the steer angle at the bottom of the screen. It still wants the maximum steer angle, just the wrong direction. Once it gets in line with this segment of line it drives straight further and further away from the B dot and cannot find the new C dot.
Turning Left:
This is not a problem because ATAN2 returns a positive value in quadrant III (over your left shoulder.) Subtraction works like it is supposed to in the abfixheadingdelta calculations.
Fixing it is much more difficult, (at least for me.) I’ve tried a lot of workarounds. Some work and some do not. Some work sometimes and when you think you have it… nope. The best success has been if the angle between where I am and where I need to be is greater than ______, set steer to max in that direction. This just continues what it was trying to do until Stanley can pick back up. You just have to be careful in getting the angle between. Same math error is there as well.
A clue would be if it works fine with a larger turn radius/wheelbase it should work just fine with small WB as well. It would seem the problem is there isn’t enough points in the uturn which are 1 meter apart. They should be about 10 cm apart. Pretty hard to do guidance on a line pointing the wrong direction as shown in the pic. Required steer angles would be undefined - as you are seeing. 1 meter between points was chosen to show the distance you have traveled thru the uturn easily, so do you want to give that a try?
Also, ATAN works properly as long as your guidance line is going the same way as you are, here:
steerAngleAB = Math.Atan((distanceFromCurrentLine * mf.vehicle.stanleyGain) / ((mf.pn.speed * 0.277777) + 1));
Also adjust this section of code in the ABLine class that prevents field tractors from busting hitches… The limit is 0.4 radians for s steering delta error which is then multiplied by the gains set - about 22.5 degrees before gains set in Stanley. Also the circular error fix won;t work well with a uturn guidance pointing the wrong direction. If the atan works fine for larger vehicles, are you sure its an atan error or maybe something else? Fun isn’t it!
//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.4) abFixHeadingDelta = 0.4;
if (abFixHeadingDelta < -0.4) abFixHeadingDelta = -0.4;
steerAngleAB = Math.Atan((distanceFromCurrentLine * mf.vehicle.stanleyGain) / ((mf.pn.speed * 0.277777) + 1));
if (steerAngleAB > 0.4) steerAngleAB = 0.4;
if (steerAngleAB < -0.4) steerAngleAB = -0.4;
steerAngleAB = glm.toDegrees((steerAngleAB + abFixHeadingDelta) * -1.0);
if (steerAngleAB < -mf.vehicle.maxSteerAngle) steerAngleAB = -mf.vehicle.maxSteerAngle;
if (steerAngleAB > mf.vehicle.maxSteerAngle) steerAngleAB = mf.vehicle.maxSteerAngle;
It sure is fun I believe in the Atan theory, but probably it does only show in Sim, because if you move antenna to 50 inch in front of pivot, it is possible to turn even with the min radius 40 inch and wheelbase at 60, although it still turns better left than right.
If you keep radius at 40 and increase wheelbase to 100, then antenna must move to at least 90
Pure genius. Changing the turn increments to 10 cm works great. I’ve not added the other code. The software can run some pretty wild combinations. I have some more checks to do. But that is a big step. I’ll keep working. Thanks.
2 Likes
Does this trick demand more computing power/ slower framerate?
I ran it on a pretty slow laptop. i could not tell it at all. What I can foresee is, have a bool to check if turn radius is less than wheel base then make this change. Or some function of it. If you want to give it a try, change line 18 in CDubins.cs to:
public static readonly double driveDistance = 0.01;
I’ve been able to crash it. But, wow, what a step forward.
Yes, it needs a uturn path it can follow. Hard to follow a path where the point step is bigger then the turn radius or wheelbase of the entire vehicle!
It is such a balance between too many points for big eq and not enough points
Not if the radius is small, there isn’t really a problem either way as the uturn is quite small. Just didn’t seem necessary at the time to have very close points.
1 Like
with these settings it works, within the field, but the distance to the edge of the field is great when turning;