Anti-tipping algorithm

[Gdeaver]

The problem
The cg of our robot is very far to the back of the robot without any totes or can. Once we are loaded the robot is very stable. At our first competition we tipped over twice. Once in auto and one time when the driver was rushing to get another RC. I was very surprised when I came to the next meeting and the programmers announced that they had implemented an anti tipping algorithm. We have a Navx mxp board on the robot . They watch the pitch and command a correcting action by reversing the drive motors if the angle goes beyond a certain value. It works! The drivers tried tipping the robot and could not. This past weekend at the Seneca District the algorithm kicked in 6 or 7 times. With out this little piece of code we could have had a disastrous weekend instead of winning. So if your robot has gone down and you have a 3 axis gyro you might look into an anti-tipping algorithm. Think inverted pendulum. High end cars have stability control now our robot does too.

[Andrew Schreiber]

Originally Posted by Gdeaver

The problem
The cg of our robot is very far to the back of the robot without any totes or can. Once we are loaded the robot is very stable. At our first competition we tipped over twice. Once in auto and one time when the driver was rushing to get another RC. I was very surprised when I came to the next meeting and the programmers announced that they had implemented an anti tipping algorithm. We have a Navx mxp board on the robot . They watch the pitch and command a correcting action by reversing the drive motors if the angle goes beyond a certain value. It works! The drivers tried tipping the robot and could not. This past weekend at the Seneca District the algorithm kicked in 6 or 7 times. With out this little piece of code we could have had a disastrous weekend instead of winning. So if your robot has gone down and you have a 3 axis gyro you might look into an anti-tipping algorithm. Think inverted pendulum. High end cars have stability control now our robot does too.

If I recall, similar code was done in 2012 by a few teams (or was part of the KoP code samples) for balancing on the bridge. Might be a place for folks looking to implement it to look if someone has the link handy.

[GreyingJay]

That's clever!

Our robot fell down a few times -- the beveled edges of the scoring platform are particularly nasty spots if you're not careful. Luckily since it's a forklift, all we need to do is lower the forks, which tips it back up!

[ToddF]

I'd love to see some video of this code in action. Theoretically, if the balance angle was known, the same algorithm would work to keep it balanced on two wheels, like a Segway.

[JamesCH95]

#teamnotippingincode

Good work!

We had a similar implementation, but were able to simplify it to an open-loop control on our arm's drive with a high success rate.

[MrForbes]

Almost like a certain type of two wheeled human transport device....





.

[Sperkowsky]

Originally Posted by MrForbes

Almost like a certain type of two wheeled human transport device....





.

Hmm I wonder what your talking about

[NotInControl]

Cool idea. For anyone that wants to try this. You don't need to buy any additional sensors, the RoboRio has a built in accelerometer, and you can use that alone to sense pitch and roll angles, depending on how the RoboRio is mounted with relation to your Robot.

Here is some code to get you started:

public static double getAccPitch() {
	
	double Y = Robot.accel.getY();
	double Z = Robot.accel.getZ();
	

	return Math.atan2(Y,Z) *180 /Math.PI;
}

public static double getAccRoll()
{
	
	double X = Robot.accel.getX();
	double Y = Robot.accel.getY();
	double Z = Robot.accel.getZ();
	
return Math.atan2(-X, Math.sqrt(Y*Y + Z*Z)) * 180/Math.PI;

}

This assumes a horizontal, mounted, upright RoboRio. You will need to modify the methods if you mount your RoboRio vertically by changing which acceleration vectors it uses.

To access the built-in accelerometer in the RoboRio simply do this:

public static BuiltInAccelerometer accel;
accel = new BuiltInAccelerometer();

The above code assumes you instantiated the accelerometer in Robot.java, but you can do it anywhere you like.

Hope this helps,

Regards,
Kevin

[Jared Russell]

Originally Posted by NotInControl

Cool idea. For anyone that wants to try this. You don't need to buy any additional sensors, the RoboRio has a built in accelerometer, and you can use that alone to sense pitch and roll angles, depending on how the RoboRio is mounted with relation to your Robot.

Here is some code to get you started:

public static double getAccPitch() {
	
	double Y = Robot.accel.getY();
	double Z = Robot.accel.getZ();
	

	return Math.atan2(Y,Z) *180 /Math.PI;
}

public static double getAccRoll()
{
	
	double X = Robot.accel.getX();
	double Y = Robot.accel.getY();
	double Z = Robot.accel.getZ();
	
return Math.atan2(-X, Math.sqrt(Y*Y + Z*Z)) * 180/Math.PI;

}

You will need to switch the methods if you mount your RoboRio vertically.

To access the built-in accelerometer in the RoboRio simply do this:

public static BuiltInAccelerometer accel;
accel = new BuiltInAccelerometer();

The above code assumes you instantiated the accelerometer in Robot.java, but you can do it anywhere you like.

Hope this helps,

Regards,
Kevin

This approach will make it appear that your robot is pitching or rolling any time you have acceleration due to any force other than gravity (such as the reactive force exerted by the carpet against the tractive force of your wheels).

[Gdeaver]

As Jared mentioned, a pure accelerometer solution is going to have noise and accelerations in it. Filtering can clean it up some what to give an estimation of gravity but the robot acceleration is still there. The advantage to the Navx MXP is that the chip handles the fusion of 3 a axis accelerometer and a 3 axis gyro.
It also can fuse a 3 axis magnetometer also but we do not use the mag. Pitch and roll are very solid and excellent for this use. Yaw is another subject. In it's current form the Navx MXP does need to be horizontal.

[NotInControl]

Originally Posted by Jared Russell

This approach will make it appear that your robot is pitching or rolling any time you have acceleration due to any force other than gravity (such as the reactive force exerted by the carpet against the tractive force of your wheels).

That is very true... the induced acceleration due to robot motion will appear as high frequency noise on each signal. The great thing about that, is that it is easy to determine which acceleration is legitimate, and which acceleration is induced due to robot motion. Simple Low pass filters, will allow for more stable readings... but will increase delay. There is a happy medium to allow for reliable tilt measurements.

A simple Euler linear filter can be used if one desires. It is a autoregressive infinite impulse response moving average.

accelX = (accelX * alpha) + (Robot.accel.getX() * (1.0 - alpha));

Where alpha is a value from 0 to 1. as you increase alpha from 0 towards 1, the filtering gets more aggressive, and the delay increases. If alpha is 0, its easy to see that there is no filtering at all.

This is the most basic filter you can implement in my book, and it does a pretty good job. There are more robust filters if you ever wanted to experiment with them. Two popular filters to try to smooth acceleration data out would be a median filter, or a Butterworth filter. They are popular because they are relatively easy to implement in code.

If you have the means to use a Gyro to measure the axis of rotation you care about, that is the sensor I would recommend for this application... if you don't, or want to experiment otherwise, it shouldn't take too much effort to use an accelerometer to have a similar implementation.

[cglrcng]

Anything to keep any bot from tipping over (that works), is absolutely great. (Glad to see others quickly share all kinds of solutions often, what helps one in this game absolutely helps all! Those QPA's will skyrocket too across the board, as will Playoff averages).

So far I have seen a ton of very nice saves, some very smart other drivers righting tipped over bots rather quickly on their alliances after mishaps that, are somewhat salvageable, and seen some actually right bots fully laying flat on their backs....Call those "The friendly emergency forklift towbots" when in trouble drivers. (Look left or right drive team when in trouble, as the AAA, emergency OnField & FieldSide Service, is actually standing right there next to you!) No need to dial 911.
_____________________


Then I asked myself...How can this apply to our team....

We have an external forklift type mainly HP fed stacker (though that isn't it's only possible usage), & the cg is quite centered unloaded). And once loaded they tip back the elevator and the entire stack for great centered cg again, line up at the scoring platform and tip the stack back forward. So tipping of robot isn't a problem...so far.

Tipping of the 6 high/capped stack is of course more than possible, if a single mecanum wheel gets up on the ramped edge, and the other one off on the carpet (they have felt that wrath and learned from it highly), and if the stack isn't really straight. But, unloading perpendicular to the scoring platform is an easy adjusted driving solution there.

As long as stack is locked properly in place (paying full attention & careful stacking and aligning totes along the way up of course, they are getting better and better w/ each match in aligning during the build).

Once loaded W/ a 6 tote high stack or 5 tote w/ RC capped laying on side, our drivers can actually do in practice fairly fast 360 spins and not drop anything and or tip over stack or bot.
_______________________________
Then I read this particular found coding/software solution thread about some who have that existing cg problem (and having seen some bots on vids have the loaded, or the unloaded, mainly unloaded cg issue in matches previously, it is a very simple & easily performed solution to the major tipping problem..."Segway Your 2015Bot" (what a tribute to Dean Kamen's 2 wheeled transport invention BTW).

I can't imagine yet, what the capabillities of the Robo Rio and plug in boards (and many different hardware/programming itterations), will be, just 3 years downline. (Already, they are nothing short of amazing).

The sharing across the board is too (and appreciated), it certainly appears to have made our first switchover year from WCD to a mecanum drivetrain much smoother it appears so far. I guess the team waited just long enough there. (Though some previous now graduated and in college students on the team, sure wanted to go that direction over the last few years I know). There was lots of talk, very little action as long as defense and pushing were keys.

I hope our team does not ever (this year), see it (this thread), and actually try it (this year). They just designed for cg loaded & unloaded, so they'd be creating huge problems I would think (fixing something that isn't actually broke), and nastily imagine...Though I could be wrong also.

I'd have extreme nightmares even thinking about the nasty possibilities of watching that tipped back fully loaded elevator start the Segway Shuffle and balancing act....That bot would surely go down in flames if it did....(Beyond looking rather a bit tipsy...as in having consumed adult beverages tipsy).
_______________________
Those installing the solution so far...Any way you developed so far, to shut it off when you do not need it? (As in when the cg is in the full comfort zone?) Such as a driver station button hardware solution, or another programming tweak solution?

Or does it just quickly correct and forget? Just wondering about an unloaded ok cg bot going quickly over the scoring platform ramps, and the possible affects of that positional vertical lean X 2 (up then down), when driving over...Does the bot get jerky there? (I just imagined one that may start dancing on the ramped portion, as it tries to quickly drive over the hump). Or one in auto hitting the ramp and "Doing the Segway...Right Into The Zone!"

My hat is off to all those that designed those plug in boards (and those adopting them), and made them avail. to all commercially the very first RoboRIO changeover year. The sky appears the limit, and the game, and timing for this game was absolutely right on there.

Early adoption of any technology is always fun. Working adoption is really fun. But, finding simple solutions to very complex problems is best.

[CalTran]

Originally Posted by MrForbes

Almost like a certain type of two wheeled human transport device....





.

Originally Posted by Sperkowsky

Hmm I wonder what your talking about

I think he means scooters.

In all seriousness though, this sounds like quite the interesting idea.

[Mike Rizzo]

Originally Posted by ToddF

I'd love to see some video of this code in action. Theoretically, if the balance angle was known, the same algorithm would work to keep it balanced on two wheels, like a Segway.

After we drop off the stack, we back up and you can see us go up on 2 wheels and stop and drop back down:

https://youtu.be/RsL2QsTnWyY?t=4h6m29s

Most of the anti-tipping saves happened in our finals matches.

[Jared Russell]

I remember 365 doing something similar in 2006.