Auto Balance for Tank Drive - Code Example?

ch1kim0n1 · March 19, 2023, 3:19am

Hi there! During the district event, I noticed that some folks had integrated an auto-balance feature for their Tank Drive robot. Since I’m currently programming a Tank Drive robot as well, I’m interested in implementing this feature too. However, I’m not sure how to get started with it.

Would you be able to provide me with a code sample and a basic explanation of how it works?

Additionally, I’m not sure if any extra tools are required to make it work, so any guidance on that would be appreciated. Thanks in advance!

kierajreed · March 19, 2023, 3:26am

We’ve been having difficulties with our auto code lately on a KoP tank chassis.

The latest iteration of it drives towards the charge station until the NavX detects a significant change in pitch, then drives up a bit more on a timer. We used to have a PID loop running on the pitch but that didn’t work so we went back to a janky time-based auto which did.

Zook · March 19, 2023, 3:30am

I’m a mechanical mentor, but since I saw this, I think I’ll take a crack.

We use the ADIS16470 gyro to get pitch angle:

github.com

Team4276/2023-Robot-Code/blob/master/master/src/main/java/frc/utilities/Gyroscope.java

package frc.utilities;

import edu.wpi.first.wpilibj.ADIS16470_IMU;

import frc.robot.Robot;

public class Gyroscope {
    public static final ADIS16470_IMU imu = new ADIS16470_IMU();

    static public double GetYaw() {

        return imu.getAngle();
    }

    static public double GetRoll() {

        return imu.getXComplementaryAngle();
    }

    static public double GetPitch() {

This file has been truncated. show original

And use that pitch to set our drive train power as proportional to the pitch.

github.com

Team4276/2023-Robot-Code/blob/master/master/src/main/java/frc/systems/Balance.java

package frc.systems;

import frc.utilities.PID;
import frc.utilities.SoftwareTimer;

public class Balance {
    private static double DEAD_ZONE = 3;
    public static boolean pause = false;
    private static final double CHECKTIME = 2;
    private static SoftwareTimer checkTimer;

    private static boolean firstRun = true;

    public static void balanceinit() {
        checkTimer = new SoftwareTimer();
    }

    public static void balance(double pitch) {
        if (pause) {
            if (firstRun) {

This file has been truncated. show original

There’s more in there that I’m not too sure about, but this worked at competition last week. The auto code folder is a bit messy, but there’s good stuff in there! Have fun browsing.

chadr03 · March 19, 2023, 3:31am

Take a look at this post. I have not tried to implement it, but it looks pretty neat and uses only the build in accelerometer in the rio.

ch1kim0n1 · March 19, 2023, 3:32am

Thank you so much, will do!

ch1kim0n1 · March 19, 2023, 3:33am

That’s a really nice tutorial, I will definitely check it out. Thank you!