Team 254 - 2014 FRC Code

[artK]

Originally Posted by magnets

I'm not 100% exactly sure how 254's code works, but I'd be willing to bet it uses something similar to what is described in this article. They can just chain the splines together and end up with a continuous and differentiable (smooth) curve because they can set the slope at the beginning and end of each segment equal.

Quintic Hermite Spline Interpolation is indeed what we used, though there was a fair bit of code dedicated to simplifying the coordinate system, so that the x axis was a straight line from waypoint A to waypoint B, and the headings were changed to account for this, though we added the condition that abs(Theta)< PI/2 so the spline can be solved for in our system. We assigned a value d to be the distance along the x axis. Attached is a picture showing the rotation and translation of the coordinate system.

We then solved for an h(x)= ax^5 + bx^4 + .... + f which has the following conditions.
h(0) = 0; h(d) = 0
h'(0) = tan(Theta A); h'(d) = tan(Theta B)
h"(0) = 0; h"(d) = 0
The coefficients then become functions of d, tan(Theta A), and tan(Theta B)

Initially, we ran cubic spline interpolation and saw that is was able to go from A to B smoothly. However, we soon realized that when we went from A to B to C there was some twitching in the robot at B. So to fix this we add the h" terms and set them to zero so the robot would always drive straight at the waypoints.

This was among the funnest piece of code I helped write, and I was super happy to see it impress most of the crowd during the Einstein finals.

[Jared]

Originally Posted by artK

Quintic Hermite Spline Interpolation is indeed what we used, though there was a fair bit of code dedicated to simplifying the coordinate system, so that the x axis was a straight line from waypoint A to waypoint B, and the headings were changed to account for this, though we added the condition that abs(Theta)< PI/2 so the spline can be solved for in our system. We assigned a value d to be the distance along the x axis. Attached is a picture showing the rotation and translation of the coordinate system.

We then solved for an h(x)= ax^5 + bx^4 + .... + f which has the following conditions.
h(0) = 0; h(d) = 0
h'(0) = tan(Theta A); h'(d) = tan(Theta B)
h"(0) = 0; h"(d) = 0
The coefficients then become functions of d, tan(Theta A), and tan(Theta B)

Initially, we ran cubic spline interpolation and saw that is was able to go from A to B smoothly. However, we soon realized that when we went from A to B to C there was some twitching in the robot at B. So to fix this we add the h" terms and set them to zero so the robot would always drive straight at the waypoints.

This was among the funnest piece of code I helped write, and I was super happy to see it impress most of the crowd during the Einstein finals.

Thanks for this description. I've played around with your code, and noticed that there when I used the cubic spline mode, there was a big jump in velocity, but in quintic spline mode, velocity was always smooth.

If I'm understand correctly, setting the second derivative equal to zero at the transition between splines and at endpoint makes it so the robot cannot be turning as it goes through the point, so the heading cannot be changing.

Would it also work if the second derivatives were equal to a value that does not have to be zero? It wouldn't be driving straight through the waypoint, but the rate of change of heading would be the same on both sides.

Solidworks has the ability to do something very similar, where points and their tangent lines are specified to generate a spline, but they have an option to "relax" the spline, which (I think) makes the second derivatives at the waypoints be equal on both sides, but lets them be values other than zero.

[Jared]

Sorry for the double post- I can't figure out how to edit the previous one.

But this is what I'm talking about- generating a spline that doesn't require the second derivative at every waypoint to be zero, like this-

http://imgur.com/d8cAypE

The left graph's spline doesn't worry about heading in the intermediate points, but does on the endpoints.

[artK]

Originally Posted by Jared

Would it also work if the second derivatives were equal to a value that does not have to be zero? It wouldn't be driving straight through the waypoint, but the rate of change of heading would be the same on both sides.

I am not sure if it would work because we never tried it, but I think it could. Setting the second derivatives to zero made sense because it ensured that it would drive smoothly, and would be easier to code. The coefficients of the spline basically became functions of the distance and the headings, all of which somebody can see. Setting the second derivatives to nonzero values also influences the coefficients, but is also harder to visualize and tune.

Tl;dr h"(0) = 0 provides a simple working solution to our problem.

[mwtidd]

// Maybe activate steering controller for dekes

Any chance you ever used this in a match?

[artK]

Originally Posted by lineskier

Any chance you ever used this (deke) in a match?

Yes, we used it during Match 27 at Central Valley Regional.

https://www.youtube.com/watch?v=P6mVJmc44mM

[notmattlythgoe]

Thanks for the negative inertia idea guys. We got it implemented and tested last night and are very pleased with the results. Have you run into any issues during competition with it that we should keep an eye out for? Have you noticed any extra drain on the battery by adding this in?

[Jared Russell]

Originally Posted by Jared

Sorry for the double post- I can't figure out how to edit the previous one.

But this is what I'm talking about- generating a spline that doesn't require the second derivative at every waypoint to be zero, like this-

http://imgur.com/d8cAypE

The left graph's spline doesn't worry about heading in the intermediate points, but does on the endpoints.

Yep, this is a common way to do splines. You generally care more that your transitions between segments are smooth than you do that they have 0 rate of heading change. But hard coding to 0 was easier

A few more notes in case anyone wants to implement something similar in the future. While we were fairly happy with the results, like all software projects, there are some cut corners and features that we never got to that would have made things better:

1) While the 2D x-y(t) splines were an easy and fun way to do this, a more common (in robotics) method of doing spline interpolation is to generate separate splines for both x(t) and y(t). This lets you deal with cases like a waypoint that is behind you, but introduces other corner cases you need to be wary of.

2) The major feature that would have been very useful is a way to detect motor saturation and adjust the velocity of the trajectory as a result. We currently generate a single S-curve velocity profile for the center of the robot over the entire length of the path, and then create adjusted trajectories for the left and right sides of the drive. This means that you need to build in a safety factor to your maximum speed, or you risk telling your outside wheels to go faster than they physically can when going around turns. The "right" way to solve this involves optimization to find the highest speed you can travel at any given point, and then working backwards to make the fastest trajectory that respects your derivative constraints while never saturating. In the "trapezoidal motion profile" case (limited acceleration, unlimited jerk), this is easy. In the "S-curves motion profile" case (limited acceleration and limited jerk), this is quite complex. Going a step further, there are approaches for iteratively optimizing the shape of the path and the speed to achieve lower total path times (ex. taking a wider turn around a corner because the robot can go faster).

3) The trajectory following controller is fairly simple and relies upon the assumption that we don't get significantly disturbed off the desired track. The trajectory is a timestamped series of positions, and when the match starts we basically hit "play" and the desired position begins advancing forward. If we were to fall too far behind, or have a huge cross track error, we would find that bad things start to happen - like short cutting, stopping way too early, or having feedforward components that are fighting against the feedback of the controller. In 2014, we required the precise timing of the trajectory to ensure we were exactly where we needed to be when it was time to fire - and with careful planning you should never encounter a disturbance like another robot (in practice, this happened a handful of times when partner auto modes didn't cooperate). In other years, a "path follower" rather than "trajectory follower" (that is, a controller that doesn't care as much about the timing information of the track, but tries to follow the spatial waypoints as closely as possible) would be a better choice.

4) Online trajectory generation from the path rather than creating waypoints files. There are approaches to do this, but ultimately we went with the files because doing numerical spline arc length integration using Riemann sums was taking too long. There are other approaches to spline arc length calculation that would have been fast enough. I am hoping that the RoboRIO's additional horsepower helps here, as well. The advantage of this (in addition to #3) is that if you find yourself disturbed off your trajectory, you can quickly compute a new one to get you back on track on the fly.

[Jared]

Currently, I'm working on implementing something similar to this while trying to reduce computing time and I had a few more questions about how yours worked. My implementation has a graphical interface for dragging points on a spline to see how it moves and constraining these points as well as a path player that lets you play through the path in real time. I started working on this a while ago, before you released the software, after talking to someone (it might have been Jared Russell?) in St. Louis about how the software worked, but now that yours is out, I've started work on it again.

If you're interested, you can check out the project here. https://github.com/dicarlo236/PathIn...nterpolate/src The cool parts are in Spline, Robot, SplineEditor, and Path. If it turns out that it generates good paths, I'm planning to post a paper explaining it later.

I originally started with the parametric x(t), y(t) splines, using two quintic hermite splines generated from basis functions here https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf (page 4). One spline was x(t), and the other y(t). I knew the desired dy/dx, so I made up values for dy/dt and dx/dt that were proportional to the desired dy/dx. The issue was that there were many solutions for dy/dt and dx/dt given a known dy/dx, and using values for dy/dt and dx/dt that were too big or too small resulted in really tight curves.

My solution was to use two short nonparametric quintic hermite splines at the beginning and end of the path and fill the middle with up to 6 cubic Bezier splines. As you guys found, you must use a higher order polynomial to be able to specify first derivative, second derivative, and position. Instead, I specified position and second derivative, which is zero at the ends of the cubic portion and equal at the knots, and let the first derivative be whatever it happened to be. This way, there's no fiddling with heading to generate a reasonable path.

The other thing I've worked on is increasing speed. I'm kind of hoping the roboRIO can do this fast enough to make updates, or to automate a short but complex driving maneuver, like our turn to line up to climb in 2013. I've gotten the calculation for a 50 ft long path down to under 200ms on my laptop (i5 @ 2.6GHz), but going from an ArrayList of pathPoints to a String for exporting takes over a second. I've also worked on simplifying the math on the splines, but have been mostly unsuccessful. I've found it a little faster, and still very accurate, to spit out 25,000 points and calculate the distance between each one, then add them up.

My other thought for speed increase is in reducing resolution. Right now, my "low-res" spline of 2,500 points updates fast enough to drag and drop individual points with a mouse and have it update very fast. Adding more points slows it down quickly. I'd like to eventually do some tests to see how small I can make the data while still generating valid paths.

Now for my questions- how does your acceleration/velocity/jerk calculation work? I've read through it a bunch of times, and I still have no idea. You have something called impulse, but I'm not sure what it is.

My current solution is way more complicated than it needs to be. Basically, it starts with the spline data (uneven spacing in distance and time, time is not known yet), and assigns max velocity to each point. Next, it calculates the radius of curvature at each point, and limits the velocity using the given max acceleration. Then, it starts at the end, sets that point to zero, then backs up, setting each one's velocity based on vf^2 = v0^2 + 2a*dx until it gets to the point where it's equal to the "unadjusted" velocity. Finally, it starts at the beginning calculating acceleration between each point given the distance between and the change in velocity, limiting the acceleration and adjusting the velocity based on the previous velocity, the distance between points, and the maximum acceleration. Interestingly enough, the jerk (da/dt) limit can be implemented by limiting dv/dx, where x is position instead. When the robot is going slow, it covers ground slowly, so the change in velocity, which is proportional to it's change in distance, happens slowly with respect to time. It the robot's going fast, the dx is being covered really fast, so the dv can happen quickly too. Next, it goes back and calculates the change in time between each point based off of the change in velocity and the acceleration at that point. At the end, it splits the trajectory into even time spaced segments, interpolating between data points where dt > 0.01.

This method has a flaw- deceleration while entering turns. If the robot's going really fast, then suddenly gets to a turn, it won't be able to slow down in time because of the acceleration limits. Right now, I either choose between ignoring the max speed through the curve or ignoring max deceleration, while still having acceleration limits coming out of the curve.

Looking through your paths, you ignored max deceleration, with accelerations less than -60, but the robot still behaved perfectly. Was this present in your final paths? If I wanted to implement a system which "looked ahead" to make sure it didn't get going too fast to be able to slow down in time(ie have a smooth function for velocity with a maximum value for dv/dt that was always below another function), how would I do this? Is this what yours does?

[apples000]

Originally Posted by Jared

Currently, I'm working on implementing something similar to this while trying to reduce computing time and I had a few more questions about how yours worked.

That sounds really cool. Any chance of a picture of it? I don't have something to build the program with at home.

As for the acceleration problem, you could locate local minimums for velocity, then work your way out of each of the mins, only going as fast as you can go. Then, when there's overlap between velocities calculated between different mins, you pick the lower one. Acceleration won't be a continuous function though, it'll have jumps. How important is jerk limits?

[AustinSchuh]

Originally Posted by notmattlythgoe

Thanks for the negative inertia idea guys. We got it implemented and tested last night and are very pleased with the results. Have you run into any issues during competition with it that we should keep an eye out for? Have you noticed any extra drain on the battery by adding this in?

Our drive code hasn't changed in a couple years. My (maybe wrong) memory is that we've discussed the drive code in previous years' 254 code release threads, and those should have some good info. I think I went into some of the design goals in one of those threads, but it has been a while.

We haven't noticed negative effects. One of the main goals of the drive code is to have the robot respond consistently exactly like the driver intends it to. Try driving the bot hard in a bunch of different scenarios and make sure you don't find anything unexpected.

[notmattlythgoe]

For anybody interested, we turned the 254 negative inertia calculations into a reusable object.

To use it you'll call the constructor with the tune-able scalar value (in 254's case 5).

NegInertiaCalc nic = new NegInetriaCalc(5);

Then call the calculate() method with the new turn value, this will return the calculated turn value.

drivetrain.arcadeDrive(oi.getThrottle(), nic.calculate(oi.getTurn()));

254 has 2 calculations depending if they have their fast turn activated or not. In this case you would just create two calculators with different scalar values.

If you have any questions or comments let me know.

[Aaron.Graeve]

I have a few questions about the Flywheel Controller used on Barrage.

I am trying to understand the states you used to control the flywheel and have gathered the second element is the velocity, but I am having difficulty understanding what the first element is. Looking back on your 2012 code, the first state seems to be a position that is taking the place of an integral term and I am guessing a similar set up was used in 2013 and 2014. Is this correct?

As I was looking for what the first element of the flywheel state is, I came across a line that was writing part of the desired state matrix.

r.set(0,0, (velGoal * (1 - A.get(1,1)))/ A.get(1,0));

Could anyone clue me into what is being predicted here?

Lastly, what method do you use to find the gains for the matrices and select the poles that will be used?

Sorry for all the questions.

[AustinSchuh]

Originally Posted by Aaron.Graeve

I have a few questions about the Flywheel Controller used on Barrage.

I am trying to understand the states you used to control the flywheel and have gathered the second element is the velocity, but I am having difficulty understanding what the first element is. Looking back on your 2012 code, the first state seems to be a position that is taking the place of an integral term and I am guessing a similar set up was used in 2013 and 2014. Is this correct?

As I was looking for what the first element of the flywheel state is, I came across a line that was writing part of the desired state matrix.

r.set(0,0, (velGoal * (1 - A.get(1,1)))/ A.get(1,0));

Could anyone clue me into what is being predicted here?

Lastly, what method do you use to find the gains for the matrices and select the poles that will be used?

Sorry for all the questions.

Sorry for taking so long. To support properly answering this question, Tom has released the control loop design code that makes this possible to follow. Thanks Tom! I helped design the controller in 2013 for that shooter based on code from 971, and Tom re-tuned it this year. The only changes (that he told me about) were to re-tune the mass for this year.

https://github.com/Team254/ControlLoops

The controller architecture used is called a Delta U controller. I haven't seen used very often, but it is one of my favorite ways of solving integral control. I learned it in a Model Predictive Control class at Berkeley, taught by Professor Borrelli.

Boiled down, a simple flywheel model would be as follows. (Check the python if my conventions for G, Kt, and Kv aren't what you expect) This model is assuming we only care about velocity, and are measuring the velocity with a velocity sensor. (banner sensor reading the pulse width)

dv/dt = -Kt / Kv / (J * G * G * R) * v + Kt / (J * G * R) * volts

A FSFB controller for that plant would apply a power proportional to the velocity error. If there is a constant disturbance, the controller will have to have an error to compensate, so it will never track perfectly.

Somewhere, we need an integrator in the system to be able to add up power to overcome this force. One classic way is to introduce a new state which is the integral of v, and stabilize that as well. This has classic integral windup problems. If everything is going according to the model, the integrator will still integrate up and cause overshoot. I don't like that tradeoff, and have been searching for a way to solve that for years.

Enter Delta U control. The basic idea is that instead of adding an integrator to the output, lets add an integrator to the input. Our state then becomes

[voltage at the motor,
velocity]

This means that our controller will output changes in power that are proportional to the difference between the goal power and the current power, and the goal velocity and the current velocity.

This wouldn't be special, except that we use the voltage estimated by the observer for the current power, and use a separate summer to add up the changes in power that the controller generates. This separate summer then commands the Talons. This means that any disturbances in the system end up showing up as a voltage difference between the voltage estimated by the observer and the voltage commanded. You can think of this like we are estimating the voltage lost due to disturbances, and adding that back into the commanded output.

The really cool part about this is that if our system is responding as the model predicts, the estimated voltage will be the same as the commanded voltage, and we won't get windup! Way cool.

Your first question was why

r.set(0,0, (velGoal * (1 - A.get(1,1)))/ A.get(1,0));

. The answer is that we need to feed in the desired state. The desired state includes velocity and power. That computation computes the steady state voltage required to go at the velocity we want. (check my math to see).

The Python is used to set the gains. The model is first tuned by plotting the system response to a step input on top of the simulated response and fitting one of the parameters. The gains are then tuned using direct pole placement, looking at the resulting values, and trying it on the robot. For this flywheel controller, the real challenge was tuning it to be robust to noise. There is a fair amount of noise from the variation in the loop frequency in Java, and more noise in the sensor reading. In 2013, we tuned it so that when the signal was noisy, the flywheel held steady. The solution here is to make the observer is very slow to filter out the noise. The controller is overdamped, which produces a pretty fast rise time, and no overshoot.

I hope this was helpful, and not too overwhelming.