We are using the pathplanner 2023-4.2, in the example shown, our robot should just rotate 180 degrees like the photo below.
However our robot rotates more than the degrees and kind of rotates back and forth to its final point. (it is pretty inconsistent and has errors of roughly +/- 5 drgs)
On top of that, translation has shown to also be an issue. We have tuned both the rotation an translation pids and they have shown little no no difference. Pathplanner believes we are driving to this position:
(I cannot add a second embedded image, however it is just a straight line)
However in reality it goes much further
We have tried small (0.25)-extremely large pid values for each. We also see if we add a second way point in between to reach our target rotation earlier, it does reach it at that point, however it continues to uncontrollably rotate as it reaches the end point.
What could be going wrong? Is the PID tuning the wrong approach? We are using the SwerveAutoBuilder. Thank you for any and all help.
1 Like
Has it worked with just a straight path? No rotation at all, just a 1-2m straight path?
Without reading your code, I recommend checking the following aspects of your robot:
- The odometry works fine. Create a Field2D object and a SwerveDriveOdometry, and just feed it the positions and angles of each module accordingly. Publish the Field2D to SmartDashboard and move it in teleop to see if the odometry makes sense. For us, this made us notice that our kinematics was wrong, as moving forwards made the odometry think the robot was moving sideways up. Also helps to ensure your gyro is correct.
- Check you gyro’s output. Make sure you are working in degrees (or the unit you want, but make sure it makes sense with every part of your code)
- Characterizing your robot might help the path planning process.
- Make sure your unit conversions are correct i.e from falcon - degrees/meters and to degrees/meters - falcon, etc with your swerve motors.
- Test the code with a 1m path. Move the swerve in auto and make it follow said trajectory, then manually measure the distance the robot has moved and compare it with the theoretical value. This can help you tune your PIDs and also will improve if using feedforward through your robot characterization
2 Likes
// Auto Stuff
SendableChooser<List<PathPlannerTrajectory>> autoChooser = new SendableChooser<>();
// SendableChooser<SequentialCommandGroup> autoChooserForNonPathPlanner = new
// SendableChooser<>();
HashMap<String, edu.wpi.first.wpilibj2.command.Command> eventMap = new HashMap<>();
public void autoChooser() {
File pathplannerDirectory = new File(Filesystem.getDeployDirectory(), "pathplanner");
String[] pathNames = pathplannerDirectory.list();
if (pathNames != null) {
for (String pathName : pathNames) {
System.out.println(pathName);
if (pathName.endsWith(".path")) {
String path = pathName.substring(0, pathName.length() - 5);
autoChooser.addOption(path, PathPlanner.loadPathGroup(path, false, new PathConstraints(1.5,0.5
autoChooser.addOption(path, PathPlanner.loadPathGroup(path, false, new PathConstraints(1.75,0.75
)));
}
}
/**
* Builds the auto command from the path group.
*
* @param pathGroup The path group to build the auto command from.
* @return The auto command.
*/
public edu.wpi.first.wpilibj2.command.Command buildAuto(List<PathPlannerTrajectory> pathGroup) {
buildCommands();
// for (int i = 0; i < pathGroup.`; i++) {
// }
SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(
poseStuff::get, // Pose2d supplier
poseStuff::reset, // Pose2d consumer, used to reset odometry at the beginning of auto
new PIDConstants(2.5, 0.0, 0.0), // PID constants to correct for translation error (used to create the X
// and Y PID controllers)
new PIDConstants(2.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the
// rotation controller)
swerveDrive::accept, // Module states consumer used to output to the drive subsystem
eventMap,
true, // Should the path be automatically mirrored depending on alliance color.
// Optional, defaults to true
swerveDrive // The drive subsystem. Used to properly set the requirements of path following
// commands
);
edu.wpi.first.wpilibj2.command.Command fullAuto = autoBuilder.fullAuto(pathGroup);
traj = pathGroup.get(0);
// holDrive = new HolonomicDriveCommand(swerveDrive, traj, poseStuff);
return fullAuto;
}
/**
* Returns the auto command selected by the auto chooser.
* Used by Robot.java to run the auto command.
*
* @return The auto command selected by the auto chooser.
*/
public edu.wpi.first.wpilibj2.command.Command getAutonomousCommand() {
return buildAuto(autoChooser.getSelected());
}
When we measure the distance it goes, it is often around 40-50% off. (when we tell it to go 3.25 meters, it goes around 5-6)
- Gyro output looks right
- Would you ellaborate on what characterizing it means?
- Unit conversions seem fine
- The translation PID doesn’t seem to be affecting anything, no matter what value we change it to.
Characterizing is getting the feedforward values for your system, in this case, your robot. This can be done through SysID. Take a look at the following links:
https://www.chiefdelphi.com/t/sysid-gains-on-sds-mk4i-modules/400373/6
https://www.chiefdelphi.com/t/applying-sysid-feedforward-to-an-sds-swerve-mk4/398455
This is a very useful video, though it is for a tank drive, but the process is the same.
Did you check your odometry readings?
At quick glance, your code seems fine.
Also, use PathPlannerServer to compare the path and where the robot think it is. This might be useful for debugging. For example, if the robot think it is pretty near the path, but physically it isn’t, it might have to do with your conversions or other factors.
1 Like
Have you checked that your gyro isn’t the issue?
