How to get the x and y speed of swerve

Technical Programming
,
1

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?

2

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
3

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

1 Like
4

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
5

image
image1039Ă—83 12.8 KB

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

6

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

2 Likes
7

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?

8

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
9

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

10

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.

11

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

12

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
13

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
14

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.

15

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
16

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

17

They do.