Help with trajectory following

Goal
My team is trying to get the robot to follow a basic path, but it’s not working. We want the robot just go straight, relative to itself.

Issue
The robot was turning by a certain amount, then driving forward. Right now, the robot turns while moving at when it’s enabled. Then, it zig-zags back towards where we told it too go.

Robot Description:
Gyro: NavX-MXP (Currently replaced with 0 values)
Motor controller: Sparkmax
Motor: NEO
Gearing: KOP gearbox
Wheels: KOP wheels

Code:
Selection from RobotContainer.java
The print statements and SmartDashboard calls where because we were trying to figure what was going on, but SmartDashboard numbers seem to be what we would expect.
Edit2: We switched -gyro.getAngle() to 0 in an attempt to reduce the number of things that could go wrong.

public Command getAutonomousCommand() {
    // An ExampleCommand will run in autonomous

    TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(2), Units.feetToMeters(2));
    config.setKinematics(drivetrain.getKinematics());
    
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
      Arrays.asList(new Pose2d(),new Pose2d(0, 1.0, new Rotation2d(0))),config);
    drivetrain.reset(trajectory);
  
    System.out.println("trajectory after reset: " + trajectory);
    var transform = drivetrain.getPose().minus(trajectory.getInitialPose()); 
    System.out.println("transofrm: " + transform);
    trajectory = trajectory.transformBy(transform); 
    System.out.println("trajectory: " + trajectory);
    
    SmartDashboard.putNumber("Initial Rotation", trajectory.getInitialPose().getRotation().getDegrees());
    SmartDashboard.putNumber("Pose", drivetrain.getPose().getRotation().getDegrees()); 

    RamseteCommand command = new RamseteCommand(
      trajectory,
      drivetrain::getPose,
      new RamseteController(2.0, 0.7),
      drivetrain.getFeedforward(),
      drivetrain.getKinematics(),
      drivetrain::getSpeeds,
      drivetrain.getLeftPIDController(),
      drivetrain.getRightPIDController(),
      drivetrain::setOutput,
      drivetrain
    );
    //drivetrain.reset(trajectory);
    return command;
  } 

The rest of the code in question is at: GitHub - frc6506/Practice-Project at 5c061a56eac6106e39a2870507790ac64b24e2db


Edit1: Spelling
Edit2: Forgot to mention thing about -gyro.getAngle().

I think your gyro angle should go here gyro.getAngle() maybe? Also don’t forget to invert that value.

I did NEO + NavX trajectory earlier, so if you need another example here is ours I was working on before the newer robot.

We switched -gyro.getAngle() to 0 in an attempt to reduce the number of things that could go wrong, but I forgot to mention it.
Edit1: Thanks for sharing your code, it’s much more elegant than ours. Maybe my team will be able to fix our issues by implementing some of the design patterns in yours.
Edit2: Spelling

Okay. I tried to follow the walkthrough tutorial as much as possible the first go through, but later when wanting to implement multiple autos, I borrowed from jdaming and 6391.

Some other issues I had where that when safety was enabled the drivetrain jittered, and so I disabled that and use drive.feed() in the setVolts(). It took me a little bit to find I needed to set the initial pose, but that was corrected in the example, and doesn’t applies here. Getting good odometry is very important, so verifying that is always good. I had problems getting everything in right units initially.

If you’re only building a Rotation2d from the gyro, you can use gyro.getRotation2d() which also accounts for inversions.

2 Likes

If I am reading this trajectory correctly, you are not telling your robot to move one meter forward, rather you are telling it to move one meter to the left. I think you want to swap the x and y values in your second Pose2d() object: …new Pose2d(1.0, 0.0, new Rotation2d(0))…

1 Like

We find that we do sometimes confuse the X and Y axis when not using pathweaver. Using the field2d() method is easy and incredibly helpful in that regard (and other troubleshooting measures).