Curve Driving Generator

[theNerd]

Ok, I've run into a great problem for all you math and programming guys. I've been working at it since December but I have made no progress. Any help would be so very appreciated. Here goes:

I am trying to create a code that will give the robot the ability to drive curves. Essentially I want to give the function a coordinate - namely a polar vector - and have it spit out the left and right speeds that the robot needs to travel to drive in a smooth curve. If there is any way possible to integrate a starting tangent line to the curve - as in say "I want a curve that crosses x and x+dx such that dy/dx @ x is equal to some number -say b" - that would be very advantageous in that I could link two curves together to have the robot not only drive in a smooth curve but also face a certain direction when it reaches its final point.

So far I have figured that we would need to consider a). the currents current speed, b). the robots current direction that it is facing and c). the arc length that the curve would need to be. I have also figured that the distance that right wheel would travel (Rd), the distance the left wheel would travel (Ld) and the speeds of the wheels (Rv and Lv) are related in this manner: Ld/Rd = Lv/Rv. ---> this relationship was based on assuming that the time for each wheel to travel a distance would be the same (and thus the velocities are different).

Note: I'm trying to keep the curves as simple as possible. For now I just want to deal with curves going to the 1st and 2nd quadrants - assuming the current robot's position is at the origin. I DO NOT want to deal with the robot driving to points to where it has to turn past (+-)PI/2 (90 degrees).

PS: Lets not discuss why we would or would not do this but instead, focus on how. Thanks!

[PAR_WIG1350]

Um... what kind of drive configuration are you using? If you are using swerve, I could probably help.

[Ether]

If you have coordinates and tangents at each endpoint, you can fit a cubic polynomial to those constraints. The result will be a smooth curve which connects the endpoints and has the desired tangent at each end.

[Hawiian Cadder]

I worked on a control scheme for a vex robot that assumes the robot is always traveling in a curve with radius R. The difference in power between the sides of the skid steer then becomes K(1/R), thus as the radius of the curve goes to infinity the difference between wheel speeds approaches 0. R had a realistic minimum in my setup (navigating a black tape maze with a light sensor) and so the algorithm worked well. For FRC it would need modification for points where R approached 0.

To achieve smooth controll I would use a PID loop where the direct control was how far side to side the robot was off course. This would control the path, planned such that the arc would cross the intended path at some fraction of the remaining distance to travel. By controlling the average of the two wheel speeds in skid steer, as wheel as the radius of the turn one could achieve any possible path. This would require closed loop motor control, as well as some method to determine where the robot actually was, as opposed to where it needed to be.

[Ether]

Originally Posted by Ether

If you have coordinates and tangents at each endpoint, you can fit a cubic polynomial to those constraints. The result will be a smooth curve which connects the endpoints and has the desired direction at each end.

Once you know the equation of the polynomial connecting the starting point and the destination, you can compute the radius of curvature (and thus the required rate of rotation) at any point along the curve.

[ewhitman]

Ether is right that the first thing you need to do is plan a path. A polynomial will work find for many situations - you need a 4th order polynomial (y= ax^3+bx^2+cx+d) to fit position and slope at both ends. However, polynomials can give you weird behavior in some situations and can't handle others - you can't get a semi-circle for example.

The math is more complex, but I would recommend looking into splines as a more versatile tool for path planning. Alternately, you could try composing your trajectories entirely of circular arcs and straight segments.

Once you have a path, you can compute the curvature of the path everywhere along it. From the path curvature and the speed, you can compute angular rotation rate (degrees/second). From the rotation rate and the speed, you can get your wheel velocities (using the geometry of your robot).

[Ether]

Originally Posted by Ether

Once you know the equation of the cubic connecting the starting point and the destination, you can compute the radius of curvature (and thus the required rate of rotation) at any point along the curve.

Attached is an example for starting point = (0,0) with a slope of 1/6
and a finish point = (3,9) with a slope of 2.

The blue line is the path and the red line is the reciprocal of the radius at each point along the curve.

If you download and install Maxima, you can play with the endpoints and see the curve that results.

Originally Posted by ewhitman

you need a 4th order polynomial (y= ax^3+bx^2+cx+d)

For the record, that's a third-order polynomial (cubic).

you can't get a semi-circle

semi-circle would be a turn of pi. The OP limited the turn to pi/2.

polynomials can give you weird behavior in some situations... The math is more complex, but I would recommend looking into splines as a more versatile tool for path planning.

If your starting and ending points are such that a single cubic results in a less-than-desirable curve, you can use intermediate waypoints to get to your destination (i.e. cubic splines). I suspect that in this application for many (most?) cases a single cubic will be satisfactory.

[dbeckwith]

I personally think that Bezier curves are the easiest to deal with, especially if you want a particular tangent on the endpoints. I would recommend reading this.
Once you understand that, programming them in shouldn't be too difficult, you would just advance the t value as the robot goes along to get the next direction you need to drive. The only challenge I really see is converting the coordinates given by the Bezier curve into drive commands, but it shouldn't be too difficult.

[theNerd]

Originally Posted by dbeckwith

I personally think that Bezier curves are the easiest to deal with

I did some research last night and I was looking at that. However, I'm not sure as to how more efficient bezier curves are to a polynomial - only testing would tell of course.

@Ether: Your idea is great! I like its simplicity, however, I do have a question: What if I don't want the robot to move in a cubic curve? Can this method still be used to create a path that looks similar to x^(1/2) - except with a bit more curve. Also, what if the current heading angle was a slope of infinity? And, instead of using matrices (as my knowledge is extremely rustic and could use some brushing up ) could I use a Taylor polynomial instead and achieve the same result - what I mean is I have no idea how to solve for the parameters using matrices.

[theNerd]

Originally Posted by PAR_WIG1350

Um... what kind of drive configuration are you using? If you are using swerve, I could probably help.

We are using a tank drive.

[Ether]

Originally Posted by theNerd

I have no idea how to solve for the parameters using matrices.

You don't have to use matrix math if you don't want to. You can use plain ole algebra.

Attached PDF has the cookbook formulas for computing a, b, c, d.

a=-(-2*y2+2*y1+(m2+m1)*x2+(-m2-m1)*x1)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

b=(-3*x2*y2+x1*((m2-m1)*x2-3*y2)+(3*x2+3*x1)*y1+(m2+2*m1)*x2^2+(-2*m2-m1)*x1^2)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

c=-(x1*((2*m2+m1)*x2^2-6*x2*y2)+6*x1*x2*y1+m1*x2^3+(-m2-2*m1)*x1^2*x2-m2*x1^3)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

d=(x1^2*((m2-m1)*x2^2-3*x2*y2)+x1^3*(y2-m2*x2)+(3*x1*x2^2-x2^3)*y1+m1*x1*x2^3)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

[Ether]

Originally Posted by theNerd

what if the current heading angle was a slope of infinity?

Pick a coordinate system whose origin is the starting point and whose positive X axis aligns with the starting direction of motion.

Compute the ending point (X₂,Y₂) and slope m₂ in that coordinate system.

Then the cubic becomes simply Y = aX³ + bX²

and the formulas for a and b are simply

a = (m₂X₂ - 2Y₂)/X₂³

b = (3Y₂ - m₂X₂)/X₂²

[Whippet]

Why not use tank control mode? That way, you can drive curves, but it takes a little more practice to use. It's just a matter of willingness to learn, but will save a lot of time programming. So it's kind of a double-edged sword.

[Ether]

Originally Posted by Whippet

Why not use tank control mode? That way, you can drive curves, but it takes a little more practice to use. It's just a matter of willingness to learn, but will save a lot of time programming.

That's not what the OP wants to discuss:

Originally Posted by theNerd

Lets not discuss why we would or would not do this but instead, focus on how. Thanks!

[MichaelBick]

Originally Posted by Whippet

Why not use tank control mode? That way, you can drive curves, but it takes a little more practice to use. It's just a matter of willingness to learn, but tank mode can do curves, and will save a lot of time programming. So it's kind of a double-edged sword.

I agree. One of the reasons that a lowered center drive(I'm assuming not 4 wheel or treads, as I would not try these maneuvers with those drivetrains) is so competitive, is because it needs very little programming. Sure, some teams have more complex code(I'm thinking 254 with their physics engine for turn radius) that is really nice, it is not worth the effort for the negligible gain, unless you have extra time(like maybe getting a camera working and shooting accurately).