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!


and

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!

Great work guys for doing this!

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”

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.

How did the web app help with development?

Thanks Tom, I look forward to reading through your 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?

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

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.


double negInertia = wheel - oldWheel;
oldWheel = wheel;

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.


double negInertiaAccumulator = 0.0;
double negInertiaScalar;
if (isHighGear) {
     negInertiaScalar = 5.0;
     sensitivity = Constants.sensitivityHigh.getDouble();
} else {
     if (wheel * negInertia > 0) {
          negInertiaScalar = 2.5;
      } else {
          if (Math.abs(wheel) > 0.65) {
               negInertiaScalar = 5.0;
          } else {
               negInertiaScalar = 3.0;
          }
      }
      sensitivity = Constants.sensitivityLow.getDouble();
 }

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.


double negInertiaPower = negInertia * negInertiaScalar;
negInertiaAccumulator += negInertiaPower;

wheel = wheel + negInertiaAccumulator;

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?


if (negInertiaAccumulator > 1) {
      negInertiaAccumulator -= 1;
} else if (negInertiaAccumulator < -1) {
      negInertiaAccumulator += 1;
} else {
      negInertiaAccumulator = 0;
}

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.

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/discussion/showthread.php/72840-Maths-acos-asin-atan

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

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:

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

https://drive.google.com/folderview?id=0BzfxWVZUp3DNREN2WWFSX21DVjA&usp=sharing

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?

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.

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.

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.

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-

Imgur

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

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.

// Maybe activate steering controller for dekes

Any chance you ever used this in a match?