Our team is using a mecanum drive train this year and have had relatively good success with it thus far. To further use the holonomic capabilities of mecanum drive, we are implementing wpilibs trajectory generation and following command designed for mecanum drive. However, we have just gotten it working but have run out of build time between now and our regional next week during week 2. This means if we are to use path following we need to tune it at our regional. So far we have been able to have the robot follow a trajectory going straight forward and stop (Not correctly tuned to distance and veers to left a little). And also drive sideways and stop (Also not tuned). What I’m wondering is if anyone has had success tuning these parameters and can share their process so we can hopefully implement it in the short time we have or if anyone has suggestions on the tuning or implementation let me know.
The PID Controllers in question:
SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.998, 2.79, 0.828);
PIDController xController = new PIDController(1, 0, 0);
PIDController yController = new PIDController(1, 0, 0);
ProfiledPIDController thetaController = new ProfiledPIDController(.95, 0, 0, new
TrapezoidProfile.Constraints(Math.PI, Math.PI));
PIDController frontLeftPID = new PIDController(.8, 0, 0);
PIDController frontRightPID = new PIDController(.8, 0, 0);
PIDController backLeftPID = new PIDController(.8, 0, 0);
PIDController backRightPID = new PIDController(.8, 0, 0);
The SimpleMotorFeedForward values were taken from the frc-characterization tool using the provided drive characterization tool.
I’m not sure how much this would help but you could take advantage of the PID controllers that run on the TalonSRX controllers instead of using WPI’s and still utilize WPI’s Mecanum controller classes. The PID constants can come from the characterization tool if you set the controller type to Talon (2020). The PID loops on the TalonSRX controllers run at 1000hz instead of the 50hz of the WPI control loop.
Instead you would use this constructor for MecanumControllerCommand. This constructor uses a Consumer that gives wheel speeds in m/s for each wheel instead of voltages. This allows you to delegate the PID to the TalonSRX but you also still take advantage of the SimpleFeedForward by passing it to the TalonSRX via arbritary feedforward. Your Consumer may look something like:
/* Note this example does not account for acceleration, which you may want to add by storing
previous timestamp and wheel speeds every loop & calculating acceleration in subsequent loops
We implemented it for differential drive with TalonSRX https://github.com/FRC-4277/2020InfiniteRecharge/blob/master/src/main/java/frc/robot/subsystems/DriveTrain.java#L237-L293
*/
(wheelSpeeds) -> {
double frontLeftFFVolts = feedforward.calculate(wheelSpeeds.frontLeftMetersPerSecond);
double frontLeftFF = frontLeftFFVolts / 12d; // Normalize to -1..1 as TalonSRX takes motor percent for arb feedforward, and they also do automatic voltage compensation
double frontLeftRotationsPerSecond = (wheelSpeeds.frontLeftMetersPerSecond) / (2 * Units.inchesToMeters(Auto.kWheelRadiusInches) * Math.PI);
double frontLeftTicksPerDs = (frontLeftRotationsPerSecond * Auto.kEncoderTicksPerRev) / 10;
frontLeft.set(ControlMode.Velocity, frontLeftTicksPerDs, DemandType.ArbitraryFeedForward, frontLeftFF)
// ... ****Repeat for other 3 wheels****
}
Another thing that may help is changing the velocity measurement period, velocity measurement window, and lowering the status frame for periods frames that handle #getSensorVelocity for more accurate and more recent velocity readings. These changes can be made in the autogenerated characterization code and your robot code. Although note the additional CAN bus utilization that may cause issues.
Thanks for pointing out that I was using the wheel radius instead of the circumference. Totally flew over my head when typing The equations out. I’ll look into utilizing the Talons controller. I’ve had success running motion profiles while utilizing the talons previously so I hope I can find the same here. Thanks for the response!