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.
Is this field-relative? How to get the field-relative one?
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.
There is also ChassisSpeeds.fromFieldRelativeSpeeds
that does this for you (by passing the reported gyro angle)
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.
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;
}
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!
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?
Yes, we currently use .02 as a constant. This should be okay but in practice a Timer is probably better.
They do.