Auto Balance using swerve and a pigeon

We haven’t tried yet but we are going to implement an auto balance function for both auton and telop. We are using team 364 swerve as a code base with some modifications.

Do we have to take in to account the direction on the field that the robot is facing? Currently we run fully field centric. Positive and Negative on the pigeon for balancing will not always be X+ and X- for the robot.

The way I am looking at this is if you have a positive value on the gyro towards the front of the robot that could be a positive or negative field movement for the robot depending on which way it is sitting.

What is the easiest was to implement this? We would never be sitting sideways to do an auto level.

Am I overthinking this and making it harder than it should be?

1 Like

Might be a slight under complication of it, but for ours we just used a simple PIDController tied to the angle of our NavX gyro. Testing at our home field its spot on 100% of the time at week 1 we still got it 90% of the time, but did notice differences between the blue and red station where we would rock more on red then blue.

// Called every time the scheduler runs while the command is scheduled.
 public void execute() {
     balanceContoller = new PIDController(BalanceP, BalanceI, BalanceD);
    // This value is passed to our drive controls methods similar to holding forward on our joystick during teleop.
    double directionforce = balanceContoller.calculate(_navXGyro.getPitchAngle(), 0);
1 Like

Make sure you do the calibration for the pigeon and save those values. I’ve noticed it helped accuracy a lot.

The sign of your pitch angle should be consistent with what way you need to drive. So once you test it interfaced to a pid controller and goes the wrong way you could flip the sign

Are you saying the first time we run it if it goes the wrong way flip it. Or every time it runs check and flip if need be?

The checking and moving we are figuring is fairly easy. It’s just the gyro positive isn’t always positive X movement unless we switch to robot centric movement while auto leveling.
Just trying to figure out the best way to do this with out driving our robot off the platform at mach 5.

Just the first time. It should remain consist each time otherwise if you think about it. As long as +X always brings you to the opposing alliance wall, and -X brings you back to your alliance wall (or vise versa).

I guess I was just trying to point out you may have to flip signs but after that it should always work.

If you are tipped downward, you’ll want to be backing up. if you are tipped upward, you’ll want to be going forward

Team 6081 doesn’t even use a PID controller. We set up and attempted to use a PID tied to the angle of the gyro, and it didn’t work at all. We tried all kinds of different tuning, changing sensitivity, changing values, and eventually we just gave up. Our solution was the KISS (keep it simple stupid) method. Instead of using a PID or even a proportional controller, our robot just drives up the charge station at a very slow speed. It stops when it hits a target angle threshold and locks the wheels in an X. So far it’s worked every single time without fail. Our lead programmer explains it in our behind the bumpers video as well.

In theory a PID should let you respond faster or handle really precise movements… but I agree. Most teams are clamping their PID anyways.

Another fun side effect of the PID is the first time you do it, not claming the output and flipping your robot off the ramp full tilt

I’m sorry I just can’t wrap my head around the fact that +x driving would always be away from us but positive Gyro tilt may not be. How does this translate to driving towards positive gyro tilt?
Like I said I think I am over thinking this.

so assuming you are driving in field relative, at least ours is setup that +X drives you down field, -X drives you up field. +X towards the other alliance wall, -X towards your alliance wall

Your going to use pitch off of your sensor. If the nose of the robot is tilted up, that’ll correspond to one sign. If the nose of the robot is tilted down, that’ll correspond to another sign.

It just depends how everything works, you may find that when your robot nose is tilted upwards (implying you need to drive forward to level), it gives you a negative pitch, in that case you’d have to flip your sign. But this should always remain consistent. If your robot is facing upwards via pitch, you’ll drive +X, if its facing downwards via pitch, you’ll drive -X. The trick is to just make sure your pitch’s sign agrees with that, and if not add a *-1 or something. You can probably also invert it in some of those sensors if you want to too

Right but what if you do this in teleop and drive up the station backwards? the sensor reading are going to be inverted but the field relative driving would not be.
Facing away from drivers front of robot up equals positive gyro tilt and then facing towards drivers with front up still equals a positve gyro tilt but you have to tell the robot to drive positve X in one example and negative x in the other example.
Auton would be easy because we can know which way the robot will be facing but in the heat of the match the driver is going to drive up which ever way is quickest.
I suppose we could say if gyro tilt is positive and gyro angle is closer to 0 degrees drive this direction and if tilt is positive and gyro angle is closer to 180 degrees drive the other way?

the sign of the gyro tilt will dictate which way you drive. I think you might be overthinking this.


  • Properly implemented field relative controls will mean a +X is always going to drive the robot towards a certain direction. The orientation of the robot should not matter. If my driver brings his joystick forward it will give the robot a +X and move the robot forward.
  • The signs of my pitch will stay consistent. It might be different for your gyro but if my robot is facing upwards, it will be a positive number. If the robot is facing downward its a negative number. It does not really matter which is which, as long as you flip the signs if the behavior is wrong when tuning.

I’m getting a little confused but are you not driving your robot field relative? Field relative would never care which direction your robot is facing.

Your gyro should also understand which axis it has been rotated in if thats your other question. If the Robot’s nose is facing up, the pitch value should be consistent regardless of what orientation its facing. Maybe that was your question

Sorry for the crude drawing but this is what I mean.
Wouldn’t you need to drive the robot differently in these 2 cases?
I think the gyro angle is the part I wasn’t thinking about. That is still going to be 0 in one direction and 180 in the other.

Maybe this is a pigeon thing but your gyro should be smart enough to understand this. Just try it out. I see what your concern is, from my understanding it will handle this for you

We can approach the Charge Station from either direction. Write the balancing assuming one side only. Then add these for complete robustness in case you decide to approach from the other side. We assume gyro yaw 0 is down field and we backup from the grid to the Charge Station (yaw 180). Adjust for your circumstance.

            if((currentYaw > 135 && currentYaw < 225))
                drivePower = -drivePower;

Yeah that’s the part that I wasn’t thinking about at first. I think that will work for us.