Hi there, We’re using the CTRE phoenix 6 swerve drive libs based on the SwerveWithPathPlanner example github project from CTRE. Everything is driving as expected, however the estimated pose and telemetry is always showing the bot moving in the opposite direction when the driver station is set to the red alliance. (both x and y)
It’s basically as if the internal code that is tracking the x/y position of the bot is not inverting it’s logic when on Red.
Has anyone else run into this? or know a simple solution?
Thanks!
When running a path? Did you update the CommandServeDrive.java to look at the alliance?
(Updated version using the PathPlanner & CTRE examples below…)
private void configurePathPlanner() {
double driveBaseRadius = 0;
for (var moduleLocation : m_moduleLocations) {
driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm());
}
AutoBuilder.configureHolonomic(
() -> this.getState().Pose, // Supplier of current robot pose
this::seedFieldRelative, // Consumer for seeding pose against auto
this::getCurrentRobotChassisSpeeds,
(speeds) -> this.setControl(autoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the
// robot
new HolonomicPathFollowerConfig(new PIDConstants(10, 0, 0),
new PIDConstants(10, 0, 0),
TunerConstants.kSpeedAt12VoltsMps,
driveBaseRadius,
new ReplanningConfig()),
() -> {
// Boolean supplier that controls when the path will be mirrored for the red
// alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
}, // Change this if the path needs to be flipped on red vs blue
this); // Subsystem for requirements
}
Yes, I made that change. The problem is not with following a path, that actually works perfect for both red and blue. The issue is with Teleop mode, The aprilTag system will detect the proper location on the field, and will call addVisionMeasurement() using the correct location. When looking at the shuffleboard field layout pose widget, the dot shows up in the exact location as expected, but as you start to drive away from the red wall, the the dot will move towards the wall, so it is internally adding to the X coordinate instead of subtracting.
CTRE’s example code doesn’t make a choice about which field coordinate system you’re going to use. Notice that the alliance supplier always returns false. As @Hollisms points out, you need to decide that and add the proper code.
Take a look at the Field coordinate system documentation. I highly recommend using Always blue origin this year, as this is what PathPlanner does (NEW this year.) If you choose to do this, you’ll also need to update the teleop driving code to deal with the alliance, as noted in the Always blue origin documentation.
CTRE support got back to me, and it turns out this is a known issue in their libs
This is a known issue that we have resolved internally but don’t have a full release for yet.
The root of the problem is that most path following programs, including PathPlanner, assume the robot’s coordinates are always referenced to Blue Alliance’s right-most corner regardless of the current alliance, and the axis pointing to the Red Alliance wall indicates positive X.
Our code currently mimics that behavior, which works great for blue alliance operation and for pathplanner autonomous (regardless of alliance), but doesn’t work if you’re teleop-controlling in red alliance with field-centric (really operator-centric) controls.
They did offer to make a dev release for me to test! so hopefully that will fix the issue.
While you wait for the release, it’s entirely possible to resolve it yourself. I think the upcoming release will be adding a feature to do more of the work for you. The library doesn’t have a bug that needs to be fixed.
To fix it, add a method to RobotContainer to determine if you need to invert the controls:
Thanks,
I’ve actually already added equivalent code, so the bot is properly driving in the correct orientation. The issue is with the internal tracking of the current pose. The estimated pose is just going the wrong way, even though the bot is traveling in the correct direction.
Then you have something else wrong. CTRE’s fix is only going to make inverting the controls easier. You might want to share your code, and be clear about if you want to use Always blue origin or Origin follows your alliance field coordinate system.
After another night of digging, I finally figured out the issue. The root cause is that our AprilTag camera is mounted on the back of the bot, and required a 2D transformation to account for this fact. I had done this already, but I was incorrectly rotating 180 degrees when on blue, but 0 on red, thinking that the fact that the bot was already oriented the opposite direction meant it didn’t need the inversion. This was not the case, it always needed the 180 degree rotation.
Thanks for all the suggestions, it definitely forced me to dig deeper into what was really going on.
