Help with auto balance code!

[taiyibah]

Hi, my drive team coach has asked for an auto code for an offseason event and although i’ve tried doing research on my own, i only seem to get nowhere. My plan is to use encoders from the motors to make the balancing easier, but all the research i’ve done on encoders has gotten me nowhere. I so far have 0% done for auto code and am just very lost in general. Anything will help!!

[Jedi_4]

use the gyro

[AndrewMcb]

I strongly recommend reading through this thread: PSA: Balance in auto

It has everything you probably need to get a working autobalance for your robot.

[ChristianFemia]

I know Team 3683 - Team DAVE released some example code for an auto balance. It uses the Rio’s onboard gyro, and should be pretty easy to use. You can find it here.

[robotmeatsuit]

Here’s the autobalance command that we used this year. worked fairly well.
You will need a gyro on the robot and a way to access its pitch from the drivesubsystem(the example uses a method called getPitch() in the drivesubsystem), as well as a method which implements arcadeDrive(lots of examples for this on the docs of WPILIB), but past that its pretty much plug-and-play.
additionally, this code is in kotlin, not java. the main difference seen here is that the init{} block in kotlin is not used in java, instead replaced by a constructor.

package frc.robot.commands

import edu.wpi.first.wpilibj2.command.CommandBase
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard

import frc.robot.Constants
import frc.robot.lib.units.*
import frc.robot.subsystems.DriveSubsystem

class AutoBalanceCommand(val drive: DriveSubsystem) : CommandBase() {
    var gyroAngle: Double = 0.0

    init {
        // Use addRequirements() here to declare subsystem dependencies.
        addRequirements(drive)
        gyroAngle = drive.getPitch()
    }

    // Called when the command is initially scheduled.
    override fun initialize() {}

    // Called every time the scheduler runs while the command is scheduled.
    override fun execute() {
        gyroAngle = drive.getPitch()
        drive.drive(
            0.0,
            Math.sin(gyroAngle) * 180/Math.PI,
            Constants.DriveMode.ARCADE
        )
    }

    // Called once the command ends or is interrupted.
    override fun end(interrupted: Boolean) { }

    // Returns true when the command should end.
    override fun isFinished(): Boolean {
        return (Math.abs(gyroAngle) < 5.0)
    }
}

[Lfehr]

What my team did was used the encoder count for distance so we knew how far we went and then to get on the charge station close to balanced and then used a PID loop to balance.

[robotmeatsuit]

there’s a gyro on the rio?

[nwfisher]

4255 used gyro data from a NavX gyro. Specifically Pitch and Roll data. Those values were used in a PID Controller and fed into a translation2d object. Our drive subsystem is based on 364’s BaseFalconSwerve, which used a translation2d, along with a few other parameters to control drivetrain movement.

If you have tank drive you could probably do something pretty similar and just use pitch to determine whether you go forward/back.

[gerthworm]

Nope.

There is an accelerometer which might prove useful though.

[robotmeatsuit]

interesting.

[BenjaminMiller]

Our auto code is interesting the way I made it. I made it so in the main drive code when the auto balance button is pressed it uses the gyro to override the joystick.

FireIslandFRC/2023FireBolt/blob/MainCode/src/main/java/frc/robot/commands/TeleopSwerve.java