A question about field oriented

Our team is recently testing our swerve drive. After we enable field oriented. It seems that our robot still has a “head”. Only the driver and the robot’s “head” face the same direction can the robot be driven in a field oriented mode. Our field oriented code is below:

    if (fieldOriented) {
        double angleRad = Math.toRadians(getGyroAngle());
        double temp = forward * Math.cos(angleRad) +
                strafe * Math.sin(angleRad);
        strafe = -forward * Math.sin(angleRad) + strafe * Math.cos(angleRad);
        forward = temp;

If there is no problem with our code . It seems that the field oriented function isn’t so useful. If not, can anybody help me with it ?

Any answer is appreciated!

A couple thoughts:

  1. Have you checked what the getGyroAngle() is returning? Does turning the robot by hand change this value?
  2. Have you checked if fieldOriented is ever true? Is it possible the body of this conditional is never being executed?
1 Like

The getGyroAngle() certainly returns.

public double getGyroAngle() {
    double angle = mNavX.getAngle() - getAdjustmentAngle();
    angle %= 360;
    if (angle < 0) angle += 360;

    if (Robot.PRACTICE_BOT) {
        return angle;
    } else {
        return 360 - angle;

     * Returns the total accumulated yaw angle (Z Axis, in degrees)
     * reported by the sensor.
     * NOTE: The angle is continuous, meaning it's range is beyond 360 degrees.
     * This ensures that algorithms that wouldn't want to see a discontinuity 
     * in the gyro output as it sweeps past 0 on the second time around.
     * Note that the returned yaw value will be offset by a user-specified
     * offset value; this user-specified offset value is set by 
     * invoking the zeroYaw() method.
     * @return The current total accumulated yaw angle (Z axis) of the robot 
     * in degrees. This heading is based on integration of the returned rate 
     * from the Z-axis (yaw) gyro.

    public double getAngle() {
        return yaw_angle_tracker.getAngle();

	public double getAdjustmentAngle() {
		return mAdjustmentAngle;
private double mAdjustmentAngle = 0;
public static final boolean PRACTICE_BOT = false;

If the getGyroAngle() doesn’t return anything , l think that l can’t even drive field oriented .

I have checked it twice , the fieldOriented is true

When I say “does it return something” I mean, have you actually seen that method print different values based on it’s orientation? I’ve seen perfectly fine code fail due to wiring issues.

Do you have this code on github? Could you share the link?

l have tested my navX through other code and it really return different value when l turn my robot. But l haven’t tested this code , but l think if my code is right , it will return the desired value.

Team6940/2021Swerve (github.com)

1 Like

I have read about the official navX’s website again . Is there some difference between the getAngle() and getYaw(). It seems that one can return value over 360, and the other can not. So, is there any problem with it?

Hello , l have checked my navx, it works well. But in a swerve drive , l can’t rotate the robot and drive it at the same time . Now , our robot only has two swerve modules . Does it have any effect with the field oriented?

Are the two diagonal from each other or on the same side of the robot?

A useful way to think about field-oriented drive is that it’s just using the orientation of the robot, relative to the field, to modify the operator controls. Typically, there are three inputs from these controls: X-translation, Y-translation, and rotation about the center of the robot. You are basically just using math to do the same thing as if the operator control was always being turned so it faces the same way as the robot.

The WPILib stuff hides the math, but this is just a transformation of the X and Y values – the rotation is unchanged. The math here is going from Cartesian coordinates (X, Y) to Polar ones (r, θ), adding some value (from the gyro) to the angle, and then converting back to Cartesian. (See https://www.mathsisfun.com/polar-cartesian-coordinates.html.)

So the number of wheels doesn’t enter into things here. In the code, functions such as hypot(), atan2(), sin(), and cos() are used to perform this math (trigonometry).

1 Like

One thing that can happen with swerve when translating and rotating is that the speeds wind up maxed out, this can sometimes interfere with rotation while translating. In WPILib, normalizeWheelSpeeds() is one way to manage this. (See https://github.com/wpilibsuite/allwpilib/blob/c1e128bd5a835a973c7c0ca2da587deef4081d26/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java#L184.)

They are diagonal from each other.

Diagonal from each other

Thanks , but when l push the xboxcontroller forward , my robot will go forward . And when l rotate 180 degrees , l still push the xboxcontroller forward , the robot will also go forward. But if l turn 90 degrees (for example) , when l push the xboxcontroller forward, the robot won’t go forward. Since l am a new to swerve , and l just want to know how will a driver drive a swerve? It seems that we need to zero the gyro? But when should we zero it? Besides , after using the field-oriented. Will the robot still have a heading? If it has a heading , it seems that it is impossible to rotate while strafe or forward. So , is there anything l need to adjust my swerve kinematics. l use Ether’s baisc swerve drive kinematics instead of WPILIB’s.

Normally, you would call a function to zero the gyro when the robot is sitting still, at the start of the match. These will usually power up at zero, but it is a good idea to explicitly zero things somewhere in your initialization code. If the zero is off and the rest of the code is working properly, what you will see is that the robot drives correctly if you hold the controller in some rotated position. So it will still make a difference which way you move the controls.

You can choose how you want the controls to operate but, normally, the swerve drive controls work just as you would expect. Basically, if you are not commanding rotation, you move the joystick in a certain direction and the robot moves in that same direction. What you are describing sounds like a bug, or perhaps more than one bug.

You can compare the code in the original post with WPILib’s Rotation2d/rotateBy:

   * Adds the new rotation to the current rotation using a rotation matrix.
   * <p>The matrix multiplication is as follows:
   * <pre>
   * [cos_new]   [other.cos, -other.sin][cos]
   * [sin_new] = [other.sin,  other.cos][sin]
   * value_new = atan2(sin_new, cos_new)
   * </pre>
   * @param other The rotation to rotate by.
   * @return The new rotated Rotation2d.
  public Rotation2d rotateBy(Rotation2d other) {
    return new Rotation2d(
        m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);

This is also covered in these videos (sorry, in English but captioning should be there): 1. How do we rotate points? and 2. Geometry of rotation.

One issue that often comes up in these types of problems in making sure that you always use a consistent coordinate system. Normally, in FRC swerve, this is based on the unit circle from trigonometry: Robot Coordinate System.

Spend some time going through your code and looking at these resources. It is often helpful to test with the robot up on blocks, and to put prints in your code to provide more visibility into what is going on internally. Even better, you can use SmartDashboard or Shuffleboard to help with this.

Thanks and here is my code,
Team6940/2021Swerve (github.com)

Can you help me check with it . l think that there is something wrong because l don’t use the Rotation2d() method.


If you don’t dig into this yourself and sort it out, you are condemning yourself to being fully dependent on others, which won’t work out well if you need to fix a bug during a competition at some point – you know your own code better than anyone else and should be faster at this than anyone else. You’re also depriving yourself of the opportunity to learn. Figuring out how to debug the code and actually going through the process will help you in many ways.

Of course, we always want to help. In this case, my sense of things is that a little push back is going to ultimately be more helpful than finding a line of code to change and sending this to you. I am confident that you can find and fix this yourself. Getting both the competence and the confidence to do this is part of the point of FIRST. It’s also a good idea to go through the code in detail with another programmer on your team. Explaining everything to someone else often helps you to understand things better yourself. 祝你好运


Sorry for responding so late. Now l know that it is really important for me. However , since l am a senior high school student. l am under great pressure of the college entrance examination. l ask this mainly because l am at school during this period of time. And l am very busy due to a lot of schoolwork and homework. However , l don’t give up , though entering FIRST does little good to the College entrance in our country. But l think this really does much good to what l will do in the future. So, really thanks to give me courage and motion to move forward on the road of FRC! Really thanks!

  • Emmm, so l just want to ask another question about field-oriented drive. Maybe it will help me dig my code.

If l accomplish the field-oriented feature, will the robot move the same as the position of the xboxcontroller. It won’t have a head?

Thanks for sharing more about your life! I hope you have a great year, and think that it’s very good to be thinking about the future, past these exams. I do know that there is a whole lot of pressure though. I do wish you the best success on the exams and in your college applications.

The point of doing field oriented is to make the robot easier to drive. What it is really doing is translating the inputs from the operator controls. Without field-oriented, moving the controls forward makes the robot move toward it’s front. With field-oriented, the robot will move toward the far end of the field, no matter which way it is facing.

To give a specific example, if the robot is turned so that it is facing the left side of the field, and there is no field oriented control, the operator has to know to command the robot to drive right, in order to move toward the far end of the field. If field-oriented control has been implemented, the operator moves the controls toward the far end of the field and the robot moves that way, no matter which way is it facing.

So, the robot does not have a head, the field has a far end. I hope this makes sense. If it does not, let us know.

If everything works as you expect when you are not using field-oriented, there’s not a whole lot more to get working in order to have this also work. But, if the robot does not drive properly when not using field-oriented, it is probably better to take care of that first, before mixing in field-oriented control.

Take a look at the link I posted before about the coordinate system for the robot. This is one area that can be confusing. Also, the inputs from many controllers are not matched up with these coordinates. So, you may have to do a transform the raw inputs from the joystick or game controller you are using. Here is a comment from the code on the swerve robot for a team I have been mentoring:

// The robot’s frame of reference is the standard unit circle, from
// trigonometry. However, the front of the robot is facing along the positve
// X axis. This means the poitive Y axis extends outward from the left (or
// port) side of the robot. Poitive rotation is counter-clockwise. On the
// other hand, as the controller is held, the Y axis is aligned with forward.
// And, specifically, it is the negative Y axis which extends forward. So,
// the robot’s X is the controolers inverted Y. On the controller, the X
// axis lines up with the robot’s Y axis. And, the controller’s positive X
// extends to the right. So, the robot’s Y is the controller’s inverted X.
// Finally, the other controller joystick is used for commanding rotation and
// things work out so that this is also an inverted X axis.

Again, get the controller inputs to work as you expect without field-oriented control, then add this in. The control inputs for X and Y are normally transformed by applying a rotation that offsets the direction the robot is facing. The control input for rotation does not normally have to be altered in any way for field-oriented control.

Thank a lot. l think that l know the field-oriented drive better. However , l have run in trouble of the motor’s internal PID parameters tuning. l think that the traditional is a little bit difficult when l tune the motor’s PID parameters. Can robot characterization tool works for it? But how can l get the KI parameter throught the robot characterization? Or, is there any other easy ways to tune it?

Is this the turning motor? I’m guessing this what you are asking about, but sometimes PID is used with the drive motor also. Are you running PID on the motor controller or on the roboRIO? If you are running it on the motor controller, what controller are you using? What kind of sensor are you using for feedback, and how is it connected? A lot of this might be in other posts, but sometimes things change over time, and it’s good to summarize when asking a new question. It’s probably better to start a new thread as well. But first, see if you can find where someone else has already asked a very similar question and read through that information.

PID tuning is a big topic, but there’s quite a lot here on CD that covers it. For swerve turning PID control, you can probably get things working pretty well with just P (I = 0, D = 0), perhaps with a little F (feedforward, based only on the direction of the error). And, you should be able to get things working reasonably well by just making small adjustments to P (and maybe F). So getting things working reasonably well and getting things really tuned in might not be the same approach. It’s useful to describe the behavior you are seeing also, instead of just asking about PID tuning without this information.