Opposite of fromFieldRelativeSpeeds?

Technical Programming
1

Is there an available method in the API that is the opposite operation of fromFieldRelativeSpeeds?

i.e. if I have a ChassisSpeed, how to convert that to field-relative X and Y speeds?

3

Doesn’t ChassisSpeed already contain field-relative x and y speeds?
vxMetersPerSecond and vyMetersPerSecond

4

No, those are robot-centric.

image
image850×150 7.13 KB

5

Apply trig to your chassis speeds using your gyroscope.

public ChassisSpeeds getFieldRelativeSpeed(double robotX, double robotY, double omegaRadiansPerSecond, Rotation2d robotAngle) {
        // Gets the field relative X and Y components of the robot's speed in the X axis
        double robotXXComp = robotAngle.getCos() * robotX;
        double robotXYComp = robotAngle.getSin() * robotX;

        // Gets the field relative X and Y components of the robot's speed in the Y axis
        double robotYXComp = robotAngle.getSin() * robotY;
        double robotYYComp = robotAngle.getCos() * robotY;

        // Adds the field relative X and Y components of the robot's X and Y speeds to get the overall field relative X and Y speeds
        double fieldX = robotXXComp + robotYXComp;
        double fieldY = robotXYComp + robotYYComp;
        
        return new ChassisSpeeds(fieldX, fieldY, omegaRadiansPerSecond);
    }

I know im misusing ChassisSpeeds by using them to store a field relative speed (that’s not its intention). 1622 has their own FieldRelativeSpeed class which is what this method is in, you pass in a ChassisSpeeds and a gyro angle and it convert it to a FieldRelativeSpeed. For the sake of simplicity I didn’t bother including that here and just modified the method to just use ChassisSpeeds.

2 Likes
6

fromFieldRelativeSpeeds rotates your velocity vector by your gyro angle to get to robot-centric. To go the opposite way you can call the same method with the negative of your gyro angle.

2 Likes
7

As in gyro angle * -1?

Or some fancy mod math?