Smooth Path Generator for RoboRio 2015

[Jared]

Originally Posted by faust1706

Ether posted a math quiz on here a little while ago that basically required someone to lay out the mathematics of how to calculate the left and right path trajectory of a robot given it's route.

http://www.chiefdelphi.com/forums/sh...d.php?t=126639

How I solved the problem was some simple calculus. I download a symbolic-numeric library that allowed me to take the derivative of my original function (the path), and then I found the function that at point x gives the slope of the normal line of my derivative function. Then I solve a system of linear equations. I want to "march out" n units from my original function, so I have the equation of the normal line and a circle around my point x (x^2 + y^2 = n^2). Then I solve the system. I do that for every point, then compile the points and fit a function to it. I feel as though there is a much better way of doing this though. (This method has proven to be quick enough for it's purpose. It takes about 10ms to go through a function with precision of 1,000 points to solve for) Could either of you explain your mathematics?

My approach is similar. There may be a faster approach, but this was simple to write, and isn't the slowest part of my program. I have a big list of about 3,000 or so points that I iterate through. I have a method to calculate dy/dx already for another part of the process, so I have that value already.

leftSegments.x = segment.x - r * sin(atan(s.dydx));
leftSegment.y = segment.y + r*sin(atan(dydx));

After this, I recalculate the displacement during the step

double ds = Math.sqrt((l.get(i).x - l.get(i-1).x) * (l.get(i).x - l.get(i-1).x) + (l.get(i).y - l.get(i-1).y) * (l.get(i).y - l.get(i-1).y))

The distance travelled (for encoder following loop)

l.get(i).distance = l.get(i-1).distance + dp;

Velocity (s.dt is the amount of time that should pass between the current segment and the previous one.)

l.get(i).vel = dp/s.dt;

Acceleration

l.get(i).acc = (l.get(i).vel - l.get(i - 1).vel) / s.dt;

and Jerk

l.get(i).jerk = (l.get(i).acc - l.get(i - 1).acc) / s.dt;

The heading is the same as the original path.