Team 254 2011 FRC Code
 

Hey all,

I'm Eric Bakan and I'm Team 254's Head of Programming/Controls aka Chief Executive Brogrammer. With the conclusion of our last offseason event, the MadTown Throwdown, Team 254 is releasing our 2011 FRC codebase.

The code can be viewed and downloaded from our team GitHub at https://github.com/Team254/FRC-2011

We had used SVN for the duration of the season but unfortunately our server died and we lost all our SVN backlogs. Based on our programming team's previous experience we've decided to move to Git for our version control, primarily because it allows us to make offline commits when we have intermittent internet access on our developer laptop. At the moment this is merely the snapshot of our code as of the end of Championships.

We hope that releasing this code will prove useful to other teams. The included README should provide useful details about the architecture of the system and the individual files should have further notes.

Our only request is that you leave a reply if you found any of this code helpful. Everything is released under a 2-Clause BSD License so feel free to use any part of it.

Re: Team 254 2011 FRC Code
 

Thanks for posting! The attention to detail is astounding. 4 different sets of PID gains for the elevator, depending on direction and number of stages engaged? :yikes:

EDIT: And that's before I even saw the state space controller for the drive :)

Re: Team 254 2011 FRC Code
 

Incredibly exciting to see such sophisticated robot code being open sourced. I would love to see other teams follow suite :)

Re: Team 254 2011 FRC Code
 

Thanks! We had a lot of fun this year. Did you also notice the threading and how we implemented Auto mode? We use trapezoidal motion profiles for all the drive commands to generate very smooth motions. We also improved the drive code so that it is even smoother and more responsive than the 2010 code. We learned a lot and will be applying what we learned to next year's code.

Drove the robot forwards for 3 seconds, spun for 3 seconds, modeled the robot from first principles, plugged in the ideal constants, fit the constants to the actual response, designed a controller in Matlab, deployed it to the robot, and it worked first try. Sooo tempting to try it for the other systems next year. I decided to model the drivetrain as MIMO. Previous attempts to model it as SISO were resulting in the bot twitching left and right a bit once it stopped due to the false assumption that accelerating one side did not result in any force on the other side.

If you or anyone else has any questions, we'd be glad to try and answer them.

Re: Team 254 2011 FRC Code
 

Thanks for posting the code Austin. I'd never heard of trapezoidal motion profiles, but the first Google link seems to give a basic understanding. I haven't been able to view the code yet, but have what assumptions about acceleration/deceleration of the manipulator parts (lift & wrist) were you able to make when modeling their movement?

Re: Team 254 2011 FRC Code
 

We used a simple PD loop on the wrist and elevator with some gain scheduling, since it worked well enough after hand tuning. The elevator changes mass when it starts to lift the second stage, so there is a second set of constants for that case. For both the arm and elevator, they would undershoot when approaching the goal from below, and overshoot when approaching the goal from above. To solve this, we have 2 sets of constants, one per direction. This solved our overshoot problems. I also avoid integral whenever possible. We just accepted the small steady state error on the elevator and wrist, and moved on, rather than spending the extra time to tune the integral constant across 6 sets of constants. We pass a goal into the two sub systems when we hit the setpoint buttons. The manual joysticks for those two systems also tweak the goals, rather than doing anything with power.

One thing we realized when driving the robot was that it was very useful for the arm to be angled ~50 degrees above horizontal when lining up to place a tube. To facilitate this, whenever a setpoint button is hit, the elevator raises up, and then when it is all the way up, the arm tilts forwards. When the driver hits another button, the robot then goes through an "auto-score" motion which opens the claw, spits out, lowers the elevator, then raises the arm back up, and backs the robot up. This offloads a lot of stuff from the drivers.

Most of the fancy controls is in the drivetrain, which we spent easily over a month working on to tune the robot so that it was accurate and precise enough to score two tubes.

Re: Team 254 2011 FRC Code
 

Ah, if only I knew enough C to appreciate it. (LabVIEW team here) I'm sure it would make me wanna tear up a bit..

Re: Team 254 2011 FRC Code
 

With that kind of automation, you can make any robot score quickly and effectively. Put it on a 254 robot and you see why they have a Championship banner.

Re: Team 254 2011 FRC Code
 

Interresting...

Is there any reason that you didn't run the elevator and arm as a single integrated system, to simplify things like this? We ran our elevator/arm as a single state machine, and part of the state transition handled actions like this (with the data input being a requested state, and the data output being a pair of setpoints), and fed the positions output into a pair of setpoint controllers and anti-death logic.

Re: Team 254 2011 FRC Code
 

The system evolved from the two separate systems with minimal automation to being connected together, and we never stopped to think about re-architecting. We also wanted to be able to move the elevator or wrist at any point in time manually and have it cancel the action.

Looking back, I really like how easy it was to write auto modes with the multi-threading that we did, and would like to make it that easy to write auto-score functions as well.

Re: Team 254 2011 FRC Code
 

Yes, multithreading can lead to prettier/less verbose auto modes, in my experience. We have been prototyping a trapezoidal motion profile as well, and I really like how smoothly it achieves distance setpoints with sub-inch accuracy. Are you using a trapezoidal profile for turning in place, as well? (I didn't notice it, but haven't gone through line by line - yet)

EDIT: Yes, I see that you did :)

Re: Team 254 2011 FRC Code
 

I perused the code over lunch and noticed something that confused me. In your ports file you've assigned both the "B" port of an encoder and a limit switch to digital I/O 10. In order to facilitate this, was there a specific order the wires had to go into the DIN for I/O 10?

We rely heavily on limit switches to act as redundant safeties during programming as well as sensor failures, so being able to pair limit switches with encoders in this manner could save us from having to make I/O port tradeoffs.

Re: Team 254 2011 FRC Code
 

The port definitions for the top and bottom roller encoders are probably obsolete. It doesn't look like either of those encoders is actually used in the code, so there's no real conflict.

Re: Team 254 2011 FRC Code
 

Correct. There are no encoders on the rollers. That sounds like a very old piece of code that didn't get removed as the code evolved...

Re: Team 254 2011 FRC Code
 

Austin,

Could you or one of your programmers explain the rationale behind the design of your victor_linearize function? You average 5th and 7th order polynomials together, but it isn't obvious why you do this.

Thanks

This question was brought on by this thread: http://www.chiefdelphi.com/forums/sh...02#post1085502

Re: Team 254 2011 FRC Code
 

Sure. I wrote this function in 2010, so I'm quite qualified to answer any questions.

Here is the data and the three polynomials that are in the function.



I generated the red data points by putting the robot up on blocks and applying PWM to the motors. I then read out the wheel speed at steady state for various PWM values.

From there, I tried to fit the data. I first started with a 5th order odd polynomial (The + and - response should be the same, which means that f(x) = -f(-x)). It is shown in green. That wasn't a great fit, so I tried a 7th order polynomial, shown in blue. Neither of them were great fits. They are not monatonically increasing functions. When you drive the robot with them, the robot doesn't feel like the throttle is a consistent function, and it feels weird (it has been a while, and feelings don't translate to words so well.)

From there, on a whim, I tried averaging the two functions. This actually turned out quite well. But, when I put it on the robot, it felt like the power was reduced too much at low speeds. To try to compensate for this, I added in a bit of y=x to get the equation shown in the legend above for the pink plot and to boost the power applied at low speeds. This is what is in use today in the victor_linearize function.

Re: Team 254 2011 FRC Code
 

Is there a reason you choose to use a polynomial function instead of a piecewise linear function?

From your empirical data it looks like no more than 5 linear sections (see red lines in figure below) would be needed to characterize the curve quite well.

This would significantly cut down on the number of computations needed to return a result, and in this case it look like it might even more accurately fit the input data.

Re: Team 254 2011 FRC Code
 

Did you do an analysis to determine whether the piecewise linear code plus the conditional logic it requires executes faster than the single polynomial ?

If speed is what you need, it's hard to beat a complete lookup table (no interpolation required):

http://www.chiefdelphi.com/forums/sh...1&postcount=12

Re: Team 254 2011 FRC Code
 

It's easier to generate the polynomial with software (completely avoiding any transcription by hand or hand calcs). This is unless there is a piece-wise linear approximation tool I am unaware of.

Also, for simple math, it's not really a concern when running on the cRio.

Re: Team 254 2011 FRC Code
 

Don't forget that another option is pre-processing of lookup tables, if the memory will hold it. 256 data points, 1 per PWM step, would be easy to calculate during sensor initialization and also cut down on cycles. Preprocessing it instead of pre-programming it also allows you to make adjustments to parameters before each match (if needed).

I agree that it's probably not needed for the cRIO, but was very useful for old-school quadrotors on primitive processors. It also helped back in the 90's with signal processing algorithms that needed to be realtime.

Theoretically one could pre-process all of the possible motor outputs versus inputs such that a lookup was done given sensor/driver input instead of a calculation -- but that's a bit overboard.

Re: Team 254 2011 FRC Code
 

Interestingly, we can specify 256 unique positions (0-255) but only a portion of them produce unique outputs on the Victor. http://www.ifirobotics.com/forum/viewtopic.php?t=317

Re: Team 254 2011 FRC Code
 

See the link in Post 18 of this thread.

Re: Team 254 2011 FRC Code
 

Can you mention the software you used to generate the plots? Is it homegrown software or commercial? The screenshot looks like ZedGraph...

Re: Team 254 2011 FRC Code
 

It's gnuplot. Free. Highly recommended.

http://www.gnuplot.info/

Re: Team 254 2011 FRC Code
 

They aren't using a single polynomial, they're using a 5th order poly and 7th order poly, then averaging the two.

The only reason I asked the question is because I looked at their code, and it seemed pretty involved for something that needs to be calculated every time you want to send an output to a motor controller.


They clearly have a handle on things, so I was wondering if this approach provided them something over what I teach my kids (the piecewise linear approach described previously)

here's the code one of my kids came up with as part of a pre-season homework assignment a few weeks ago. "scale" is a 2d array of points characterizing the piecewise function. The benefit of implementing it this way is that you can modify your array of points at any time, to tweak behavior, without having to modify your code.
Anyone who's been through an Algebra 1 class should be able to quickly grasp the math. Added bonus of them getting to apply existing classroom knowledge.

Re: Team 254 2011 FRC Code
 

No tool is needed, you simply identify the X and Y coordinates of points that define the lines.

My team dumps these points into a 2d array and uses the point slope formula to calculate the output for any given input.

Agreed, we could pre-compute a lookup table, but unless we compute for all possible inputs we'll have to quantize the output or interpolate between the points.

The main reason why we don't take this approach on my team is because of the flexibility provided by the piecewise function. We can code our method once and set it up to operate on a 2d array of x,y coordinates. Changing that array is SUPER simple if we need to do so later on, and won't require any changes to the supporting code.

Re: Team 254 2011 FRC Code
 

Given how overpowered the cRIO actually is, this doesn't use much compute power. It is only calculated 200 times a second (Could be a different amount, it has been a while). We don't come close to running out of compute power, so why worry?

Once we had a working solution, we stopped working on it. There are more numerically efficient ways to get a similar answer, but we moved on to more important problems once we had a working answer. Now that I look at it in more detail, we could probably cut out half the compute cycles in the function without much work (Bring it down to around 10 multiplies, probably even less). You can read my description above for how I came up with the functions themselves. They really aren't that hard to fit.

I'm a big fan of smooth motion. I don't like corners, or corner cases. Unless there were a good number of segments to the piecewise fit, it would have "corners" that I could "feel" as I drive it. When I tried driving with the original 5th order polynomial, I could feel the oscillations in the result and didn't like it at all. And hand fiddling with the piecewise function to get it to feel nice and match well would be as much work as the polynomial. Also, I read Adam's statement as saying that trying to automatically fit piecewise lines to the points is a lot harder than just fitting a polynomial. If you look at the source data that I started from, it is a bit noisy. Just interpolating between the points would be sub-optimal.

Re: Team 254 2011 FRC Code
 

What do you get when you add two polynomials together? They could have gotten the same result with a single polynomial:

If P is a 7th degree poly, and R is a 5th degree poly, then Q = a*P+b*R is a 7th degree polynomial ("a" and "b" are constants).

I was trying to point out that it is not immediately obvious that a piecewise linear function "would significantly cut down on the number of computations needed to return a result" compared to a single polynomial. I inferred, based on the context, that you intended the word "computations" to imply "processing speed" and include not just arithmetic but conditional logic and branching as well.


By the way, if you use two nx1 arrays instead of one nx2 array it saves cycles computing the indices. And if you pre-sort your [x[],y[]] points so they are in increasing order by x[], you can save a few cycles in your table lookup logic by eliminating one of the tests. Assuming x[0,1,..n] and y[0,1,...n] are tables of X and Y data values, and [x[n],y[n]] is a sentinel point to assure the function always returns a value:

for (i=1,i<=n,i++) if (ax<x[i]) return y[i]+(ax-x[i])/(x[i-1]-x[i])*(y[i-1]-y[i]);

Depending on the compiler, it might save a few more cycles to use pointer arithmetic to scan the x[] table.

Re: Team 254 2011 FRC Code
 

When I was looking at the code and converting it to LabView, I realized that there are 3 calculated constants that you could replace if you quickly wanted to speed up the function. Both victor_fit_a1 and victor_fit_a2 are constants that are calculated, that you could replace with actual constants Also for the answer function .85*.5 can be combined if you need a little extra resources.

Re: Team 254 2011 FRC Code
 

Those calculations are done at compile time, not run time. Changing them will not speed up the code.

Re: Team 254 2011 FRC Code
 

In light of the fact that the Victor's inputs are highly discretized by the Victor itself (see http://www.vexforum.com/showthread.php?t=20350), a small lookup table (mapping PWM duty cycle<->speed) will ultimately provide the best possible accuracy, and will be computationally inexpensive to run. If you look under the hood of the WPIlib PWM code, all continuous-valued speed commands are ultimately converted to a PWM duty cycle value between 1 and 255 (0 is a reserved value for holding the line low). The effective duty cycle for the Victor is then .665ms + X*6.525us, where X is the PWM value. Since there are only 255 unique values, at most you need 255 elements in your lookup table (and in actuality, the beginning and end of this list will be past the Victor's 1.0 to 2.0ms allowable input range spec anyhow).

Define a float/double array with 255 values. The index is the PWM value; the value is the actual speed for that value. To populate this array you could, for example, run a "sweep" of the Victor by running a motor at each possible value for a couple of seconds and recording the total distance traveled during each segment (you would want to filter this data to preserve monotonicity). It would take several minutes to run, but you only need to do it once (when I get a chance, I will take an old bot and do exactly this, and post the results).

Driving the Victor would be accomplished by searching through the 255 value array for your desired speed and selecting the index of the closest value in the array. Using a nearest-value binary search (since the array ought to be sorted) would require only 9 comparisons*, worst case, to select the optimal PWM value from the array.

Using this method, you are guaranteed to have the least possible linearization error, though the polynomial method is close enough that I doubt you'd notice the difference.

*At most 8 comparisons are necessary to search a 255 element array (since worst case complexity is O(log2(n)), but you would want an extra comparison to check if the adjacent value in the array is actually closer, since the easiest way to write a nearest-value binary search would be to find the closest value that is always either > or < the input.

Re: Team 254 2011 FRC Code
 

Would this work?

- Apply joystick commands -127 to 0 to 127 and measure the Vic's output voltage¹ for each one.

- Normalize those measured voltages to the (floating-point) range -127.0 to 0 to +127.0

- From this data, for each desired whole number output (in the range -127 to +127), determine (by simply looking at the measured data)² the integer value required to come nearest to the desired output. Put these integers into an array (-127 to +127).

- The index of the array is the output you want (-127 to +127), and the value stored at that index is the value you should command. No lookup search or interpolation required.



¹ stalled? or free? probably makes a difference. not sure whether you want average or rms voltage.

² or just let a spreadsheet do it for you

Re: Team 254 2011 FRC Code
 

The only downside I see to this approach would be a "loss of precision". Say PWM duty cycle 150 produces a speed of 74, PWM 151 has a speed of 74.5, PWM 152 has a speed of 75. If you wanted to command a speed of "74.5", there is an exact PWM value for that speed. But by quantizing the available speeds into whole number outputs, you would end up never using PWM 151.

If that isn't splitting hairs, though, I don't know what is.

Re: Team 254 2011 FRC Code
 

I feel the need to quote this to emphasize it. Someone on 254 this year had a huge lookup table to do just this, but, as far as I could tell, didn't filter the data. The result was a bit inconsistent, and we went back to the polynomial.

To do it really right, I want to take a victor, battery, CIM, and a dynamometer, and generate data points for various different operating points. (Use 16 speeds and 16 PWM values.) Not sure where I can get access to a dynamometer though. I have noticed that in practice, for example, if you are going forwards, and apply a tiny bit of backwards power, the entire drivetrain locks up.

Re: Team 254 2011 FRC Code
 

If you have a CAN Jaguar and a spare motor lying around, I'd imagine you could use the Jag's current sensor as a makeshift dyno.

The drive locking up when you apply a bit of reverse power makes perfect sense given that the motor controller has an internal H bridge. When you switch directions, it's as if you are in "brake mode". This can be very useful for stopping on a dime in auto mode.

Re: Team 254 2011 FRC Code
 

Hi,
Thanks for posting your code. The architecture is very pleasing to a controls engineer. I like control loops and filters as classes.

Can you expand a little on your use of negitive inertia. I assume this is equivalent to adding derivatives of the control input to improve performance, but I would like to hear more about your use. Any papers to refererence?

Also, I think you can use much simpler functions to fit your Victors. One linearizer that involves a single parameter to adjust is:

vic_cmd = min_cmd/(1 - (1-min_cmd)*input_cmd)

where min_cmd = the minimum command to keep the motor moving, i.e. equivalent to the torque required to overcome dynamic friction.
All commands are normalized [-1,1]. One can play with min_cmd to get a very good fit. The equation is valid for input_cmd>0. You have to change a few signs for input_cmd<0 and the min_cmd can be different for each sign.
This equation follows directly from the steady state current model of the h_bridge with a low frequency pwm. The current model is summarized in this older post. I will write a blog post on this later.

Attached is a curve fit of some old data I had on a victor. Maybe Joe Ross sent it to me and it could be an 883 or 884. The inverse is obtained by taking the victor speed and running it through the inverse function above.