Problems running the WPILib RamseteCommand example

[Michael]

My team is trying to implement Ramsetes pathing this year. We are using C++, the Command robot style, and have TalonFX motors and a Navx gyro.

Following the example using RamseteCommand class here
We are having problems getting it to work: the robot just blasts backward at max speed for a few seconds and stops.

The main differences between our robot and the one in the example lie in our TalonFX’s having internal encoders that don’t map onto the WPILib encoder objects, and the Navx gyro not mapping onto the example’s gyro. However, we seem to be getting that info into the odometry object correctly, as driving the robot around in teleop with the odometer info on the smart dashboard has the angles, X, Y, and wheel speeds displaying the right values (based on driving around a gym floor with convenient foot-square tiles). The earlier robot characterization steps in the tutorial seemed to work well and got good data for determining the needed constants.

The most concerning thing is that when this example’s auto is triggered and the robot goes zooming backward instead of doing the nice “S”-curve it’s supposed to, the dashboard does not update. After the robot stops, values eventually start to trickle in. It’s unclear if the example is skipping the Periodic() calls that update everything, or if the command has somehow locked up the whole RIO. Doing normal autos, the dashboard acts the same as if in teleop: so there’s something fundamentally wrong here, not so surprising that the PIDs can’t work with the data if the dashboard can’t. Our code is all available here.

Any suggestions for where to look to figure out what’s going wrong?

Also, one technical coding question: why is the example ramseteCommand object being fired off with the “std::move()” thing? Other command helper objects in our Autos.cpp file are simply added to a big command group. Does anyone have examples we could look at of this object being used in the normal command-based auto routine rather than hijacking the end of the RobotContainer code (the way the example does for simplicity)?

Thanks in advance for any suggestions!

[Peter_Johnson]

The issue likely lies in your DriveSubsystem’s ResetEncoders() and GetLeftEncoder()/GetRightEncoder(). What’s important to realize with CAN motor controllers is that unlike encoders on the Rio, their encoder reset() method takes some time to propagate (as it’s sent over CAN to the motor controller) before it’s seen by the encoder get() method (which is read from motor controller telemetry). The easy workaround for this is to never actually reset the motor controller encoder but instead store an offset when it’s “reset” and subtract that offset when reading the encoder value.

[Michael]

Thank you for your suggestion, and sorry for the delayed reply: the build team gave us the new robot this week, so getting it set up took over.

We changed the reset to use the offset variables as you suggested. That plus inverting the voltages in TankDriveVoltages did the trick! The robot will now attempt to follow an S curve!

As we try to get command sequences built now, a question: unlike other commands we build based on the CommandHelper class, why is the RamseteCommand invoked with the std::move() in the example:

frc2::SequentialCommandGroup(
      std::move(RamsetesCommand),
      frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {} ));

rather than directly like:

frc2::ParallelRaceGroup{ShooterShoot(shooter,NULL),
                              TurnToLimelight(drive).WithTimeout(2_s)},

in an AddCommands() list?

[Peter_Johnson]

It’s because it’s getting constructed on a separate line versus constructed and passed directly as a rvalue. std::move turns a lvalue (what normal variables are when used in an expression) into a rvalue. If the std::move wasn’t there, it would copy the variable instead of moving it. See std::move - cppreference.com

[Michael]

Thank you so much for all of your help!