How to get the x and y speed of swerve

l use WPILib to control my swerve and l want to get the x and y speed of my swerve in auto, how can l do that. In the teleop, it seems that l can get these two values from chassisspeed, but in auto, which uses setModulestates() to control. So, can l get the x and y speed from swervemodulestates. Or , is there any method to convert the swervemodulestate to the chassisspeed again so that the x and y value can ge got?

You can create module states using the swerve encoders and do the transition from module states to chassis speeds and then use the gyro to convert to field-oriented values.

2 Likes

Is this field-relative? How to get the field-relative one?

1 Like

The function generates robot-relative speeds, you will need to use trigonometry to convert the speeds.
I would start with combining the x and y speed to create your robot-relative speed vector, then rotate to be field-centric using the gyro angle and finally split it again to field-relative x and y speeds.

2 Likes


l think these two lines of code may solve the problem.

There is also ChassisSpeeds.fromFieldRelativeSpeeds that does this for you (by passing the reported gyro angle)

2 Likes

Thanks , l have found some bugs about how to get the field-relatie speed recently. Thats if l use the code below to get the field-relative speed, will the exceed the limit( The max speed a swerve can reach)

    vxMetersPerSecond =       
      ChassisSpeeds.fromFieldRelativeSpeeds(
        translation.getX(),
        translation.getY(), 
        omega, 
        GetGyroRotation2d()).vxMetersPerSecond;

    vyMetersPerSecond =       
      ChassisSpeeds.fromFieldRelativeSpeeds(
        translation.getX(),
        translation.getY(), 
        omega, 
        GetGyroRotation2d()).vyMetersPerSecond;

 SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.kMaxSpeed);

Since these two methods to get the field-relative speed are executed before the desaaturateWheelSpeeds, so will the vMeterpersecond and vyMeterpersecond exceed the max speed? l think it may, so how to avoid that?

Actually–are you trying to get field-relative speeds FROM robot-relative speeds?

If so, this is how we did it. I think 1706 did the same thing when they were doing shoot-on-the-fly stuff.

1 Like

Great! Thanks! Another little question, are the vxMeterpersecond and vyMeterpersecond in the Robot Coordinate system?

I believe so; per the chassis speeds docs, positive vx is forward and positive vy is left. Pretty sure field-relative speeds use the field coordinate system? I’m not entirely sure, I’d ask a WPILib maintainer.

Thanks for replying! l think @calcmogul or other people may know whether field-relative speeds are in field coordinate system.

Yeah this is how we did it.

Created a field relative class to make it clear its not a “ChassisSpeed” anymore, although its similar. Apologize in advance if the formatting gets messed up.

public class FieldRelativeSpeed {
    public double vx;
    public double vy;
    public double omega;

public FieldRelativeSpeed(double vx, double vy, double omega) {
    this.vx = vx;
    this.vy = vy;
    this.omega = omega;
}

public FieldRelativeSpeed(ChassisSpeeds chassisSpeed, Rotation2d gyro) {
    this(chassisSpeed.vxMetersPerSecond * gyro.getCos() - chassisSpeed.vyMetersPerSecond * gyro.getSin(),
            chassisSpeed.vyMetersPerSecond * gyro.getCos() + chassisSpeed.vxMetersPerSecond * gyro.getSin(),
            chassisSpeed.omegaRadiansPerSecond);
}

public FieldRelativeSpeed() {
    this.vx = 0.0;
    this.vy = 0.0;
    this.omega = 0.0;
}

Then in our Drivetrain subsystem we put the following in the periodic loop

m_fieldRelVel = new FieldRelativeSpeed(getChassisSpeed(), getGyro());

and added the getter function getFieldRelativeSpeed

public FieldRelativeSpeed getFieldRelativeSpeed() {
    return m_fieldRelVel;
}
1 Like

Unrelated to the thread, but how does your FieldRelativeAccel class look? I believe I saw it on your shoot while move thread and am interested in how you calculated it. Thanks!

1 Like

It’s basically (New-Old)/time. Although we did put a cap at 6m/s2 and 4rad/s^2 to help somewhat, once it worked pretty well we left it alone. This kinda just helped filter spikes that we knew weren’t possible but there is almost certainly a better way.

Hopefully by mid next week we will do a full code release. And will be doing a specific CAD release (first time our team has used onshape for a design) for the new simple turret gearbox for the Armabot Turret 240 that @Bigathedestroyer designed.

The way I thought of doing it would be (currentSpeeds - oldSpeeds) / 0.02 to account for the loop time and get to m/s^2–would your “time” variable be the loop time in the same way as my thought?

1 Like

Yes, we currently use .02 as a constant. This should be okay but in practice a Timer is probably better.

They do.