Swerve control - a bit too "jumpy"

Technical
1

We’re testing our swerve prototype with field-centric swerve, and found that controls are a bit too “jumpy”, especially “left” and “right”, as well as holonomic rotation, especially if it’s paired with the directional control, where more fine adjustments are usually needed.

We tried Logitech joysticks and XBOX controllers, and also implemented CUBE driving like shown in Nu23, with more fine control on the lower end of the joystick move.

We’re simply trying to make it easier for drivers to navigate swerve and have better control over it.

Do you have any suggestions on the controllers as well as better algorithms to supply directional/rotational input? For instance, perhaps, you found better luck in limiting the holonomic rotation to 50% of the max rotation speed?

Our swerve prototype is capable of 2-3 RPS stationary holonomic rotation at max speed, and directional maximum speed around 10-12fps.

2

100 has had good luck with a sort of “throttle” that specifies the full-range speed. I think there are 2 or three speeds to select from, e.g. 25, 50, 75, and 100 percent of max.

you also might want to look at the low level control loops to see what their error is-- if you’re supplying infeasible input to the low level then it can seem “jumpy,” meaning full of lag and overshoot.

1 Like
3

You may also want to consider SlewRateLimiter. We also decoupled translation speed from direction with a gas pedal (the trigger) for translation speed. We just slowed open loop rotation down to 1/3. There is also a slow mode. This implementation could be cleaner

package frc.robot.commands;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.LogitechController;
import frc.robot.constants.GlobalConstants.ControllerConstants;
import frc.robot.subsystems.Drivetrain;

public class GamepadDrive extends CommandBase {
	private Drivetrain m_drivetrain;
	private LogitechController m_gamepad;
	private SlewRateLimiter xLimiter = new SlewRateLimiter(3);
	private SlewRateLimiter yLimiter = new SlewRateLimiter(3);
	private SlewRateLimiter rotationLimiter = new SlewRateLimiter(3);

	/**
	 * Constructor method for the GamepadDrive class
	 * - Creates a new GamepadDrive object.
	 */
	public GamepadDrive(Drivetrain drivetrain, LogitechController gamepad) {
		super();
		addRequirements(drivetrain);
		m_gamepad = gamepad;
		m_drivetrain = drivetrain;
	}

	@Override
	public void execute() {

		double throttle = modifyAxis(m_gamepad.getRightTriggerAxis());

		if (m_gamepad.getLeftBumper()){
			throttle = throttle/3;
		}


		double translationX = modifyAxis(-m_gamepad.getLeftY());
		double translationY = modifyAxis(-m_gamepad.getLeftX());
		if (!(translationX == 0.0 && translationY == 0.0)) {
			
			double angle = calculateTranslationDirection(translationX, translationY);
			translationX = Math.cos(angle) * throttle;
			translationY = Math.sin(angle) * throttle;
		}

		m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
				-Drivetrain.percentOutputToMetersPerSecond(xLimiter.calculate(translationX)),
				Drivetrain.percentOutputToMetersPerSecond(yLimiter.calculate(translationY)), getRotationRadiansPerSecond(),
				m_drivetrain.getYawR2d()));

		SmartDashboard.putNumber("Drive Rotation", getRotationRadiansPerSecond());
		
		/*
		 * m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
		 * getXTranslationMetersPerSecond(),
		 * getYTranslationMetersPerSecond(), getRotationRadiansPerSecond(),
		 * m_drivetrain.getGyroscopeRotation()));
		 */ }

	@Override
	public void end(boolean interrupted) {
		m_drivetrain.drive(new ChassisSpeeds(0.0, 0.0, 0.0));
	}

	private double getRotationRadiansPerSecond() {
		return -Drivetrain
				.percentOutputToRadiansPerSecond(rotationLimiter.calculate(modifyAxis(m_gamepad.getRightX(),2))) / 3;

	}

	private static double modifyAxis(double value) {
	
		return modifyAxis(value, 1);
	}
	private static double modifyAxis(double value, int exponent) {
		// Deadband
		value = MathUtil.applyDeadband(value, ControllerConstants.DEADBAND);

		 value = Math.copySign(Math.pow(value, exponent), value);

		return value;
	}
	
	private double calculateTranslationDirection(double x, double y) {
		// Calculate the angle.
		// Swapping x/y
		return Math.atan2(x, y) + Math.PI / 2;
	}

	
}
1 Like
4

Does that still allow the driver to exert full power in a moment?

5

It just makes the change a bit slower. It won’t prevent you from “jamming the throttle” but it may feel slightly delayed. But I would imagine it’s very barely.

My car has the same thing on the throttle and it’s not noticable with normal driving

6

You can’t compare driving your car and driving an FRC robot… You need to give the driver the ability to make hard maneuvers to avoid collisions

7

I guess you’ve never driven in Chicago :rofl:

5 Likes
8

@Dave1998 With a 3 in the SlewRateLimiter it means you can reach 3 in 1 second. 1 being 100% so in effect it makes it take 1/3 of a second to reach 100% output. a small limit. We did limit rotation though and the driver has no way to full throttle rotate.

2 Likes
9

drivers don’t generally like the effect of the slew rate limiter due to the lag it creates, and especially the overshoot. note some joysticks have a round response shape, i.e. the “lower right” corner doesn’t produce x=1, y=1, but rather x=0.7, y=0.7. so if you cube the input you’ll get a sort of “cross” shaped response with low output in the corners.

1 Like
10

“some” being almost all of them. In the end it doesn’t really matter for swerve drives because the radius is still 1

11

This is exactly why you use a SlewRateLimiter it doesn’t impede driver control much but will take some getting used too, the delay is not enough to cause issues with control normally. They are also very important to prevent robots with bad center of gravities from falling over, however if the center of gravity is variable you may want to implement velocity limiting based off of an estimated center of gravity and momentum.

12

imo the driver should be good enough to not need a limiter. They should know not to let go of the stick when the robot is at full speed and they should have the ability to change directions at a moment’s notice to avoid collisions.

13

I had never considered that being an issue with cubing. What is the easiest way to avoid this problem?

14

You could also have a throttle map, such that most of the stick’s travel results in slow output.
For example 80% x input on the stick only gives you 50% output.

15

The CUBE driving kind of works like that - the X% of the stick gives Y% of the output while the full throttle still gives the full output.

I will check the SlewRateLimiter

16

Do you have a code for

percentOutputToMetersPerSecond

that you can share? That can clarify how do you calculate the actual speed.

Using one joystick (or set of them) to set a direction and another to control the throttle is indeed intriguing. We will look into that.

17

It is the max drivetrain speed in meters per second * the percentOutput

	public static double percentOutputToMetersPerSecond(double percentOutput) {
		return DrivetrainConstants.maxSpeed  * percentOutput;
	}

	public static double percentOutputToRadiansPerSecond(double percentOutput) {
		return DrivetrainConstants.maxAngularVelocity * percentOutput;
	}
1 Like
18

Needless to say that this is very very inaccurate and should only be used for teleop driving

19

How else would you calculate percent out to meters per second.

Now one’s method for deriving the maxSpeed may be inaccurate, but this is pretty much the reference formula for percent output to speed

20

Back-EMF causes the relation to be non-linear to the best of my knowledge.