Balancing on the charging station with swerve

Technical Java
1

For starters i should note that I am following team 364’s falcon base swerve code using mk4i modules.

I have a rough idea of how a command to balance your robot on the charging station would work in theory. Give your pidcontroller a setpoint (something like 0 degrees pitch) and then have it calculate an output based off of your error then feed it to your drive method. Within actual application this should be simple enough with tank system using a differential drive method to control your drive base.

Im confused on what would be the swerve equivalent of feeding your pid calculated values to your drive method since for translation control (forward and strafe) it accepts a translation2d value instead of a double. This is the exact method for reference:

public void drive(
Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) {
SwerveModuleState[] states =
DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getRotation2d())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation));

SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeedMetersPerSecond);

for (SwerveModule mod : modules) {
  mod.setDesiredState(states[mod.moduleNumber], isOpenLoop);
}

}

Any help/ideas are greatly appreciated.

2

The Translation2d is a way to store your desired velocity in 2 components (the X and Y direction). Since your code can run in field oriented mode, you can just set the fieldRelative parameter to true and then input your desired velocity into the Translation2d parameter. Once you have that, you can take a look at the WPIlib coordinate system and figure out which way is forward (is it positive X or Y???). Then you make a Translation2d out of that and pass that in. Since you will be moving forward and backward relative to the field when balancing on the Charge Station, you want to put your calculated PID values into the forward/backward component of the Translation2d and set the other component to 0 (unless you want to go sideways for some reason).

If you need more specific code examples please let me know, I don’t want to spoonfeed and just give you code but I can help guide you through this.

(you would pass all this into the drive method btw, if that isn’t clear)

3

You could also do it robot-centric. Assuming you have an IMU, you can get the pitch and roll angles, and convert them to a net tilt and the robot-centric x-y direction of that tilt. Then drive uphill.

4

I would greatly appreciate any sort of guidance you could offer, I tried listening to what you said and tried mocking up a command:

Screenshot (64)
Screenshot (64)1575×971 50.2 KB

Id love to know what you think because I dont really know if this is right or not and the chassis is currently being worked on so I have no means of physically testing it rn. Jusgt would like to know if it checks out in theory.

5

That’s pretty close, the only mistake is that you’re using PIDController wrong. forwardController.calculate() takes in the current measurement and the setpoint, not the error. It can calculate the error on its own. The correct way to write this would be like this

@Override
public void execute() {
    currentAngle = driveTrain.getGyroPitch();
    output = MathUtil.clamp(forwardController.calculate(currentAngle, Settings.BALANCE_BEAM_DEGREES_GOAL));

    driveTrain.drive(new Translation2d(output, 0), 0, true, true);
}
6

oh wow I literally did it like this first but then i saw other people do it using error and i thought i was doing it wrong. Turns out they werent using a pidcontroller which is why they manually used error. Anwyays that was extremely helpful thank you.

7

yep youre welcome! lmk if you need anymore help

8

I’m in a similar situation and I’ll be honest, I’ve done little with PID, and so lifted somone’s driveto code, and tried to retool it for balance in 2 dimensions. When I trigger the bot on a level surface it tries to go crazy. Maybe I just overcomplicated it, or just am not getting the nuance…

package frc.robot.commands;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.SwerveDrivebase.Swerve;
import frc.robot.subsystems.runtimeState.BotStateSubsystem;

public class Balance extends CommandBase {

private static final TrapezoidProfile.Constraints X_CONSTRAINTS = new TrapezoidProfile.Constraints(3, .5);
private static final TrapezoidProfile.Constraints Y_CONSTRAINTS = new TrapezoidProfile.Constraints(3, .5);
private static final TrapezoidProfile.Constraints OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints(2, .5);
private final ProfiledPIDController xController = new ProfiledPIDController(3, 0, 0, X_CONSTRAINTS);
private final ProfiledPIDController yController = new ProfiledPIDController(3, 0, 0, Y_CONSTRAINTS);
private final ProfiledPIDController omegaController = new ProfiledPIDController(2, 0, 0, OMEGA_CONSTRAINTS);

private final Swerve drivetrainSubsystem;

/** Creates a new Balance. */
public Balance(Swerve drivetrainSubsystem) {
this.drivetrainSubsystem = drivetrainSubsystem;

xController.setTolerance(2);
yController.setTolerance(2);
omegaController.setTolerance(Units.degreesToRadians(3));
omegaController.enableContinuousInput(-Math.PI, Math.PI);

addRequirements(drivetrainSubsystem);

}

@Override
public void initialize() {
}

@Override
public void execute() {
yController.setGoal(0);
omegaController.setGoal(drivetrainSubsystem.getYaw().getRadians());

var xSpeed = xController.calculate(drivetrainSubsystem.getRoll());
if (xController.atSetpoint()) {
  xSpeed = 0;
}

var ySpeed = yController.calculate(drivetrainSubsystem.getPitch());
if (yController.atSetpoint()) {
  ySpeed = 0;
}

var omegaSpeed = omegaController.calculate(drivetrainSubsystem.getYaw().getRadians());
if (omegaController.atSetpoint()) {
  omegaSpeed = 0;
}

drivetrainSubsystem.drive(
    new Translation2d(ySpeed, xSpeed).times(BotStateSubsystem.MaxSpeed),
    omegaSpeed,
    true,
    true);

}

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

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

9

Team 5031 mentor
Our robot has a simple kit of part chassis and uses the DifferentialDrive class with an arcade drive method. The team purchased a Kaui lab ( navxmxp) sensor. Wishes to balance the robot on the charge station. I examined the Kaui lab example for balancing the robot, but it uses the MecanumDrive class. Any suggestion on how the team would be able to accomplish balancing on the charge station? My knowledge of programming is limited. We are using Java-based programming.

10

some basic steps after determining how to drive aboslute directions ( forward, backwards , left, right )

1 ) figure out a set point or target for your bot to be at , determine a safety threshold or limit
2 ) if navx pitch readings exceed your threshold or limit in positive , drive positively ( forwards )
3 ) if navx pitch readings exceed your threshold or limit in negative drive negatively ( backwards )
4 ) if navx roll readings exceed your threshold or limit in positive , drive positively ( right)
5 ) if navx roll readings exceed your threshold or limit in negative drive negatively ( left )

bind the above to a button , once you have that logic working , implement a PID loop of some kind to finetune.

1 Like
11

The problem with this is that when using a differential drive, there is no “drive right” and “drive left”. Holonomic drives allow independence of forward/back and left/right, but with a differential drive, what you have are speed and turn rate.

However, the basic idea is still perfectly ok. Instead, though, you can read the yaw value, and use it to make sure that you are perpendicular to the middle of the charging platform, and then use the pitch value to drive forward and back.

Doing just that, I managed to make it work, sometimes, on our practice field element. Next I am going to introduce some dependencies on pitch change, instead of just pitch value. Without it, it has a disturding tendency to overshoot, and drive off the opposite side of the platform.

12

Hello Everyone
My knowledge is limited in programming. I created the following drivetrain subroutine:

import com.kauailabs.navx.frc.AHRS;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.PS4Controller;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class DriveTrain extends SubsystemBase {

MotorControllerGroup leftMotors;
MotorControllerGroup rightMotors;
DifferentialDrive drive;
AHRS ahrs;
boolean autoBalanceXMode;
boolean autoBalanceYmode;
static final double kOffBalanceAngleThresholdDegrees = 10;
static final double kOonBalanceAngleThresholdDegrees = 5;
private final AnalogInput rangefinder;
/** Creates a new DriveTrain. */
public DriveTrain() {
CANSparkMax leftfront = new CANSparkMax(Constants.leftfront, MotorType.kBrushless);
leftfront.setInverted(true);
CANSparkMax leftback = new CANSparkMax(Constants.leftback, MotorType.kBrushless);
leftback.setInverted(true);
CANSparkMax rightfront = new CANSparkMax(Constants.rightfront, MotorType.kBrushless);
rightfront.setInverted(false);
CANSparkMax rightback = new CANSparkMax(Constants.rightback, MotorType.kBrushless);
rightback.setInverted(false);
leftMotors = new MotorControllerGroup(leftfront, leftback);
rightMotors = new MotorControllerGroup(rightfront, rightback);
drive = new DifferentialDrive(leftMotors, rightMotors);
rangefinder = new AnalogInput(Constants.channel);

}

@Override
public void periodic() {
// This method will be called once per scheduler run
}
public void driveWithjoysticks(PS4Controller controller, double speed) {
drive.arcadeDrive(controller.getRawAxis(Constants.PS4Right_Yaxis)*speed,controller.getRawAxis(Constants.PS4Left_Xaxis)*speed);

}
public void driveForward(double speed){
try (AHRS gyro = new AHRS(SPI.Port.kMXP)) {
//Shuffleboard.getTab(getName()).add(gyro);
double kp =1;
double error = - gyro.getRate();
drive.tankDrive(speed + kperror , speed - kperror );
}
}
public boolean drivetodistance (double distance, double speed){
while (rangefinder.getAverageVoltage() > distance)
{
driveForward(speed);
}

  return true;
}

public void angle (){
try (AHRS gyro = new AHRS()) {
//Shuffleboard.getTab(getName()).add(gyro);
double kp =0.05;
double error = 180 - gyro.getRate();
drive.tankDrive(kperror , - kperror );
}

  }   
  public static PS4Controller driveJoystick;
   public boolean operatorControl (double xAxisRate, double yAxisRate){


drive.setSafetyEnabled(false);

while (operatorControl(xAxisRate, yAxisRate) && isEnabled()){

  driveJoystick = new PS4Controller(Constants.Joysticknumber);
  
    double pitchAngleDegrees    = ahrs.getPitch();
    double rollAngleDegrees     = ahrs.getRoll();
if ( !autoBalanceXMode && 
     (Math.abs(pitchAngleDegrees) >= 
      Math.abs(kOffBalanceAngleThresholdDegrees))) {
    autoBalanceXMode = true;
}
else if ( autoBalanceXMode && 
          (Math.abs(pitchAngleDegrees) <= 
           Math.abs(kOonBalanceAngleThresholdDegrees))) {
    autoBalanceXMode = false;
}
if ( !autoBalanceYmode && 
     (Math.abs(pitchAngleDegrees) >= 
      Math.abs(kOffBalanceAngleThresholdDegrees))) {
    autoBalanceYmode = true;
}
else if ( autoBalanceYmode && 
          (Math.abs(pitchAngleDegrees) <= 
           Math.abs(kOonBalanceAngleThresholdDegrees))) {
    autoBalanceYmode = false;
}

// Control drive system automatically, 
// driving in reverse direction of pitch/roll angle,
// with a magnitude based upon the angle

if ( autoBalanceXMode ) {
    double pitchAngleRadians = pitchAngleDegrees * (Math.PI / 180.0);
    xAxisRate = Math.sin(pitchAngleRadians) * -1;
}
if ( autoBalanceYmode ) {
    double rollAngleRadians = rollAngleDegrees * (Math.PI / 180.0);
    yAxisRate = Math.sin(rollAngleRadians) * -1;
}

}

// Called every time the scheduler runs while the command is scheduled.

return autoBalanceXMode;
}
public synchronized boolean isEnabled () {
return this.isEnabled();
}

public void stop ()
{drive.stopMotor();}
}
I get the following error: at frc.robot.subsystems.DriveTrain.operatorControl(DriveTrain.java:89)

13

Please encapsulate your code with three back ticks ` to make it readable for future reference