Team 254 - 2014 FRC Code
 

Hi everyone,

We would like to share the software that controlled Barrage in its quest for the 2014 FRC Championship.

Our robot ran Java this year as it did in 2013. To develop Barrage, we added a bunch of new features like waypoint navigation that drives smooth splines, an internal web server for debugging controllers and modifying robot constants, Cheesy Vision, single threaded autonomous scripting, and more. Check it out!

https://github.com/Team254/FRC-2014
and
https://github.com/Team254/TrajectoryLib

If you have any questions please do not hesitate to ask... We love talking about our work! Also please feel free to try/learn from/re-use this code as much as you see fit.

Thanks!

Re: Team 254 - 2014 FRC Code
 

Great work guys for doing this!

Re: Team 254 - 2014 FRC Code
 

Thank you so much for posting these; they're really an inspiration for our programming team. The trajectory generator and spline classes are really, really awesome.

I do have a few questions:

1. What is the negative inertia accumulator in Cheesy Drive? I've seen it in previous code releases, but I have no idea what it does.

2. Why didn't you guys use the included math libraries? I'm not following your code entirely.


BTW, I love the rainbow "Teh Chezy Pofs Deta Lawgr"

Re: Team 254 - 2014 FRC Code
 

Typically when you command a robot to rotate and then let go of the joysticks, it may continue to turn slightly due to inertia. The negative inertia in Cheesy Drive reduces (prevents?) this turning overshoot and makes the robot more intuitive to drive.

Re: Team 254 - 2014 FRC Code
 

How did the web app help with development?

Re: Team 254 - 2014 FRC Code
 

Thanks Tom, I look forward to reading through your code.

Re: Team 254 - 2014 FRC Code
 

Do you have any overall documentation on what the code is actually doing, a high level description document? It's not abundantly clear looking through the code how it all works, and what and why it's actually doing it (although in general it looks very well put together, partitioned, and coded).

Some questions:
What are the waypoints referencing and how were/are the derived?
What input is used to select a trajectory?
Are the trajectories/waypoints only used during autonomous or can you somehow pick a way point during teleop and minimize driver work-load?

Re: Team 254 - 2014 FRC Code
 

Yeah, we can put something together. We really just wanted to get this out before the summer and haven't quite had the bandwidth to put this together yet.

The waypoint system is for autonomous only. Basically, you can specify a list of waypoints where the start of the path is (0,0,0) in (X,Y,Theta). We calculate a spline that intersects each waypoint at the desired angle. From there we generate a set of (Position, Velocity, Accel) for each wheel at 0.01 second time steps along that spline. This calculation is done off board as the spline formation is pretty cpu hungry. We serialize the data and load it on to the crio as a text file which gets deserialized. The trajectory construction happens in the TrajectoryLib project in Main.java. The trajectories get deserialized and stored in memory in AutoPaths.java.

In autonomous, we load a path into the TrajectoryDriveController and set the Drivebase subsystem's active controller to that. (Each subsystem can have one active controller which controls it).

We do some driver off-load things, but mostly for the operation of the intake and shooter subsystems (for example there is a button that deploys the intake and runs automatically until we sense we've picked up the ball, we only shoot for certain presets once the wheel is settled at speed, etc).

Re: Team 254 - 2014 FRC Code
 

Ok Tom, you're going to have to help me understand some of these negative inertia steps as I really like the idea a lot.

Here you calculate what the negative inertia is between the last turn iteration and the the current one. So if you are turning full left and decide to stop the negInertia is 1 and the old turn value is now 0. Understood so far.
Next you determine your negative inertia scaling value based on high or low gear. Understood because you'll be turning faster in a higher gear and the robot is not going to want to slow down as quickly in high gear so more drift meaning a higher scaling value is needed. Still following.
Next you calculate what power you're going to need by multiplying your negative inertia by the scaling value and add this to the accumulator value. Then you add this value to your turn value to counter the inertia. Still following.
This is where you lose me. From what I understand from this next bit of code you are increasing/decreasing the inertia accumulator by 1 to get it closer to zero, if it is -1 <= x >= 1 then you set it to 0. This makes sense as you'll want the negative inertia to get smaller as the robot slows down. Where I'm lost is the fact that negInertiaAccumulator is never used again in this method, and it's a local variable so its lost after the method is complete. This means on the next run through of this method all of the negative inertia work is lost and the correction is only done for one iteration. What am I missing?

Re: Team 254 - 2014 FRC Code
 

That trajectory stuff is so cool. No wonder you guys are world champions. And it makes the actually robot code so much more simple. We might actually look into doing something similar but even for just straight lines. Should allow much more accurate autos. Plus the simpler the robot code the better.

Re: Team 254 - 2014 FRC Code
 

I don't know why they didn't use the included math libraries, which AFAIK would return the exact same values as theirs does, but "their" code comes from here.

http://developer.nokia.com/community...acos-asin-atan

Re: Team 254 - 2014 FRC Code
 

Good catch. That's a bug. The original C++ code from 2011 had the accumulator as a member variable.

Re: Team 254 - 2014 FRC Code
 

I've been playing around with the trajectory planning software, and I'm working on an application to quickly input waypoints, visualize output, then FTP to cRIO.

Right now, I'm got it set up so that I can input a bunch of waypoints and have it spit out graphs of robot position, and the trajectories are really amazing. The path is smooth and makes sense, and robot motion is super smooth.

There is one possible bug that I've seen. Sometimes it makes a path that travels the correct x and y distance, but doesn't start at (0,0). Usually it does. Maybe I'm just not understanding it right?

The only thing I haven't been able to make it do is a full 180 degree turn.

See some pictures here:
http://imgur.com/a/Q4RPw

The program-a work in progress- (which is fun to play with):

https://drive.google.com/folderview?...&usp=sha ring

How exactly does the calculation for finding the splines work? Would there be a way to modify it so that you can enter a waypoint that is just a position, and has no heading?

Re: Team 254 - 2014 FRC Code
 

So one of the caveats of the Trajectory generator that I forgot to mention is that it does not account for motor saturation. If you try to make a turn too quickly and the motors cannot keep up, it will go off path. We had thought about building something that could fix the cross track error but it was just easier to make safe paths.

Re: Team 254 - 2014 FRC Code
 

I'd like to try your program, but the file linked isn't working for me.

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.

Re: Team 254 - 2014 FRC Code
 

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.

Re: Team 254 - 2014 FRC Code
 

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.

Re: Team 254 - 2014 FRC Code
 

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.

Re: Team 254 - 2014 FRC Code
 

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.

Re: Team 254 - 2014 FRC Code
 

Any chance you ever used this in a match?

Re: Team 254 - 2014 FRC Code
 

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

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

Re: Team 254 - 2014 FRC Code
 

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?

Re: Team 254 - 2014 FRC Code
 

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.

Re: Team 254 - 2014 FRC Code
 

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?

Re: Team 254 - 2014 FRC Code
 

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?

Re: Team 254 - 2014 FRC Code
 

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.

Re: Team 254 - 2014 FRC Code
 

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).

Then call the calculate() method with the new turn value, this will return the calculated turn value.
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.

Re: Team 254 - 2014 FRC Code
 

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. 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.

Re: Team 254 - 2014 FRC Code
 

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 . 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.