Keeping field Field Relative after path planner auto

Our team is using swerve drive for the first time this year. So far we have been very successful with it. However since we believe we made enough district points to make districts for the first time since 2019 we are looking to fix some of the gaps we have.

We are using PathPlanner for Auto and have been very pleased with the results. Our problem comes in that at the start of telop our driver has to turn around and zero out the gyro to allow for field relative drive. We are not using vision is there anything we can do to allow path planner starting position to carry over into telop. I have included the code that allows for field relative drive and zero gyro. Also a link to our github.

  var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
        fieldOrientation
            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
                Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
            : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
  public void zeroHeading() {
    m_gyro.reset();
  }

Github Code

Does this problem only occur when you are on the RED alliance? If so, this is the expected behavior and you need to update your teleop code to handle it.

PathPlanner uses the “Always blue origin” approach to the field coordinate system. You can read more about this in the WPILib documentation. In short, when you’re on the blue alliance the driver wants the robot to drive in the positive direction on the field’s X axis. But, when you’re on the red alliance the driver wants pushing forward on the joystick to drive in the negative X direction.

You didn’t share your teleop code, but you probably want to invert the driver’s translation controls when you’re on the red alliance. There’s an example in the WPILib docs I linked above.

EDIT: Sorry, you did share all of your code. I missed the link at first. You have a command in RobotContainer that’s processing your joystick input. You’ll want to invert driverLeftStick.getY() and driverLeftStick.getX() only when you’re on the BLUE alliance.

1 Like

this happens if we are on the Red or Blue alliance.

below is the code we use for driving. Because we are currently using the Gyro we are not inverting for when we are the red alliance.

m_robotDrive.setDefaultCommand(
                // The left stick controls translation of the robot.
                // Turning is controlled by the X axis of the right stick.
                new RunCommand(
                                () -> m_robotDrive.drive(
                                                -MathUtil.applyDeadband(driverLeftStick.getY(),
                                                                OIConstants.kDriveDeadband),
                                                -MathUtil.applyDeadband(driverLeftStick.getX(),
                                                                OIConstants.kDriveDeadband),
                                                -MathUtil.applyDeadband(driverRightStick.getX(),
                                                                OIConstants.kDriveDeadband),
                                                false),
                                m_robotDrive));
}

There are several footguns potentially.

  1. Where you initialize your odometry in DriveSubsystem, there is a overload for starting pose.

  2. When you specify the modules chassis offsets the order can be important for which side is the “front”.

  3. Mentioned PathPlanner always blue origin.

  4. Mentioned inverting the drive commands X & Y joystick inputs if keeping blue origin.

  5. How the robot is lined up, facing towards or away from drive station (may have to utilize initial pose here)

  6. Otherwise resetting the gyro heading. Pathplanner may do this (not sure about that) but if you have it resetting before/after auto maybe that is an issue.

It’s a little tough to figure out without knowing more about the behavior, but I have a feeling it’s because you’re using the gyro angle, not the estimated pose’s angle for teleop.

When you’re using odometry or pose estimation, you should never be resetting your gyro angle without also resetting your estimated pose, so that was a hint that something wasn’t right.

What you probably want to do is change your DriveSubsystem.drive() method to use the estimated pose instead of the gyro:

var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
    fieldOrientation
        ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
            getCurrentPose().getRotation())
        : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));

Additionally, if you want to keep your zeroHeading() method, you should update it to also set the heading on the pose estimator. Something like this:

public void zeroHeading() {
  m_gyro.reset();
  poseEstimator.resetPosition(
      getGyroscopeRotation(),
      getModulePositions(),
      new Pose2d(getCurrentPose().getTranslation(), new Rotation2d()));
}

I also recommend removing the call to m_gyro.reset(), but it shouldn’t matter if you want to keep it.

This can be a problem if you are on the Red alliance. Per the documentation, ChassisSpeeds.fromFieldRelativeSpeeds() wants a CCW positive angle that is zero when facing away from the Driver Station wall. If you are on the Red alliance, your current pose would be 180 degrees facing away from the DS wall (unless you are mirroring the field).

What can be a problem? Are you commenting on my response, or just reiterating what I stated earlier?

Sorry, I could have been clearer there…

If your robot pose is always relative to the 0,0 origin on the Blue alliance side of the field, using getCurrentPose().getRotation() for fromFieldRelativeSpeeds() would have you going backwards when you are on the Red alliance. If your pose is flipped for Red, it will work as is.

Ok, yeah, that’s just reiterating the info I shared earlier.

We have the same issue. The way we work around this when we are on the red alliance is a quick and dirty fix by just flipping the gyro 180 degrees on teleop init when we are red.

 if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red){
      RobotContainer.driveSubsystem.invertGyro();

Are you using YAGSL? We are using YAGSL and it does not have an invertGyro() function. I would prefer to fix our odometry this way rather than inverting controls, but can’t figure out how to make it work with our code.

Sorry, should have clarified. We created a method called invertGyro() that just adds 180 to the current yaw.

That workaround might work for you, but I wouldn’t recommend it to others. If they are doing anything with odometry or pose estimation (including AprilTags), this will cause more issues than it will solve.