Team 254 2011 FRC Code

[apalrd]

Originally Posted by AustinSchuh

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.

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.

[AustinSchuh]

Originally Posted by apalrd

Is there any reason that you didn't run the elevator and arm as a single integrated system, to simplify things like this?

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.

[Jared Russell]

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

[JesseK]

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.

[Alan Anderson]

Originally Posted by JesseK

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.

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.

[AustinSchuh]

Originally Posted by Alan Anderson

The port definitions for the top and bottom roller encoders are probably obsolete.

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

[Jared Russell]

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

[AustinSchuh]

Originally Posted by Jared341

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

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.

[otherguy]

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.

[Ether]

Originally Posted by otherguy

This would significantly cut down on the number of computations needed to return a result

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

[otherguy]

Originally Posted by Ether

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

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.

double RobotState::victor_linearize(double goal_speed)
{
	const double deadband_value = 0.082;
	if (goal_speed > deadband_value)
		goal_speed -= deadband_value;
	else if (goal_speed < -deadband_value)
		goal_speed += deadband_value;
	else
		goal_speed = 0.0;
	goal_speed = goal_speed / (1.0 - deadband_value);

	double goal_speed2 = goal_speed * goal_speed;
	double goal_speed3 = goal_speed2 * goal_speed;
	double goal_speed4 = goal_speed3 * goal_speed;
	double goal_speed5 = goal_speed4 * goal_speed;
	double goal_speed6 = goal_speed5 * goal_speed;
	double goal_speed7 = goal_speed6 * goal_speed;

	// Constants for the 5th order polynomial
	double victor_fit_e1		= 0.437239;
	double victor_fit_c1		= -1.56847;
	double victor_fit_a1		= (- (125.0 * victor_fit_e1  + 125.0 * victor_fit_c1 - 116.0) / 125.0);
	double answer_5th_order = (victor_fit_a1 * goal_speed5
					+ victor_fit_c1 * goal_speed3
					+ victor_fit_e1 * goal_speed);

	// Constants for the 7th order polynomial
	double victor_fit_c2 = -5.46889;
	double victor_fit_e2 = 2.24214;
	double victor_fit_g2 = -0.042375;
	double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2 + victor_fit_g2) - 116.0) / 125.0);
	double answer_7th_order = (victor_fit_a2 * goal_speed7
					+ victor_fit_c2 * goal_speed5
					+ victor_fit_e2 * goal_speed3
					+ victor_fit_g2 * goal_speed);


	// Average the 5th and 7th order polynomials
	double answer =  0.85 * 0.5 * (answer_7th_order + answer_5th_order)
			+ .15 * goal_speed * (1.0 - deadband_value);

	if (answer > 0.001)
		answer += deadband_value;
	else if (answer < -0.001)
		answer -= deadband_value;

	return answer;
}

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.

    static double getInterpolatedAxis(double input){
        for(int i=0;i<scale.length-1;i++){
             if(input<scale[i][0]&&input>scale[i+1][0]){
                double slope=(scale[i+1][1]-scale[i][1])/(scale[i+1][0]-scale[i][0]);//get slope
                double intercept = (-1*slope*scale[i][0])+scale[i][1];
                return (slope*input)+intercept;
            }
        }
    }

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.

[AustinSchuh]

Originally Posted by otherguy

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

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.

[Ether]

Originally Posted by otherguy

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

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.

[AdamHeard]

Originally Posted by otherguy

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.

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.

[JesseK]

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.