Auto balancing command

I’ve been wondering about the charging station and how to engage it in autonomous. I’ve written some code here and I was wondering if anyone could check behind me?

My thought process was that I could use a wpilib PID controller to control the pitch of our robot on the charging station. With this code, I thought that with a setpoint of 0 and the control variable being the pitch axis reading of a 3 axis gyroscope, the robot would try to balance itself based on the gyroscope readings until a certain boolean condition is met.

Something like this:

public CommandBase balanceCommand() {
        return run(
            () -> tankDrive(
                Constants.k_BalanceController.calculate(
                    getRobotPitch(),
                    0
                ),
                -Constants.k_BalanceController.calculate(
                    getRobotPitch(),
                    0
                )
            )
        ).until(
            () -> (getRobotPitch() < 0.1 && getRobotPitch() > -0.1 && m_IMU.getGyroRateZ() < 0.5)
        );
    }

Is my theory sound here or am I way off?

1 Like

That’s about the measure of it, yeah.

You can make the controller a method local of the factory and capture it rather than making it static, or you could persist it in the subsystem. A controller isn’t a constant!

You can also use the controller API to detect the end condition, instead of your custom check.

2 Likes

Would you recommend using the controller API instead of the the .until() decorator? Would that improve functionality?

I would recommend using the controller API with the until decorator. You pass a condition to until. Can you see any method on the PIDController API that you could use to check that condition?

1 Like

One of your sides is negative, so this command will rotate in place. also calling a PID controller twice in one function will mess up the d component, so make sure to only call the controller once and use that result in both tank drive sides.

just be careful your robot does not go to fast and fall off the edge, a max speed would be helpful.

1 Like

thanks! I totally would have missed that!

1 Like

could I use the atSetpoint() and setTolerance() methods?

No problem, thanks for sharing your code!

1 Like

yep!

1 Like

Don’t forget to not drive off the station. If another robot is mounting the station or you fail to account for the reaction time of the pivot, you could end up driving off the high side.