Swerve Auto Balance Code Examples?

Hello all, my team is looking for examples of how you can do an auto balance with swerve drive.

We are struggling to get it ours work, simply explaining the workflow would be just as helpful.

1 Like

This was ours last week. You can improve upon it I’m sure but it met the basic need to dock or engage. Though the pitch values may be robot dependent. Someone suggested the slow speed ~1 ft/s. For us the fast speed was ~7 ft/s. Also, you should zero your heading in auto init.

Here is our code, we have a proportional scaler based on the pitch value of the robot.

// RobotBuilder Version: 5.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

// ROBOTBUILDER TYPE: Command.

package frc.robot.commands.SmartCommands;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import java.util.function.DoubleSupplier;

import frc.robot.commands.DrivetrainCommands.Drive;
import frc.robot.commands.DrivetrainCommands.DriveAmount;
import frc.robot.subsystems.drivetrain;
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
import frc.robot.subsystems.wrist;


    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS

/**
 *
 */
public class Balance extends CommandBase {

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
        private final drivetrain m_drivetrain;
        private double pitch;
        private double speed;
        private double threshold = 5;
        private double distance = 10;

 
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS


    public Balance(drivetrain drivetrain) {


    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES

         m_drivetrain = drivetrain;
        addRequirements(m_drivetrain);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    }

    // Called when the command is initially scheduled.
    @Override
    public void initialize() {
    }

    // Called every time the scheduler runs while the command is scheduled.
    @Override
    public void execute() {
        pitch = m_drivetrain.getPitch();

        for(int i = 0; i<3; i++){
            if(pitch < 0){
                new DriveAmount(m_drivetrain, distance, speed, true);
            }
        }
    }

    // Called once the command ends or is interrupted.
    @Override
    public void end(boolean interrupted) {
    }

    // Returns true when the command should end.
    @Override
    public boolean isFinished() {
        return false;
    }

    @Override
    public boolean runsWhenDisabled() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DISABLED
        return false;

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DISABLED
    }
}

Also, Am I embedding code correctly? Kind of new to posting on here.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.