So my team is working on getting motion profiling working on our robot using the RamseteCommand from wpilib. I have rebuilt the example code and successfully integrated it into our own code. In doing some testing over the past couple of days we have been noticing the drive train makes some clicking noises as it moves through a simple straight trajectory. Upon closer inspection, the clicking is a result of the motors suddenly and briefly going in the wrong direction. When we plotted the voltage that was being fed to the drive train, it was clear that, throughout a trajectory, every once in a while, the RamseteController would feed the motors a negative voltage. This error was repeatable, we’d notice these points of error at the same points on the curve every time we ran the path.
A little more information about our drivetrain, we are using three Falcon motors on either side, with the motors on each side being contained within a SpeedControllerGroup that is being fed the voltage. This error only occurs when we are trying to use the RamseteController, as the clicking is not apparent when we are normally driving or running a simple bang-bang controller.
We have tried using follow mode on the TalonFXs instead of SpeedControllerGroups to no avail, .setSafetyEnabled(false) is being called on our differential drive, and adding differentialDrive.feed() to the tankDriveVolts method did not help either, as mentioned in another post. We are currently running paths generated from pathweaver, but have also tried using custom trajectories, similar to the RamseteController example, but still experienced the same issue. The TalonFX firmware is up to date. WPILib is up to date. We’ve even tried changing the TalonFX’s to TalonSRX’s in the code, but that also didn’t help, nor did feeding a speed controller a converted percent value instead of a voltage. So far we have only tried driving in a straight line. The problem also persisted when we tried simply running the example code. We also reran the robot characterization tool to ensure that the constants we were using were good.
Needless to say, it’s been a long day.
Anyway, here’s our code: Drive class ProfileDrive class - Uses the RamseteCommand
Other possibly relevant classes in the project include Constants and Encoders
One thing I noticed in your code is you’re using DifferentialDrive, which inverts the right side output, meaning your right side encoder will be out of phase. I don’t think I see anywhere where the encoder value is inverted.
Ah, didn’t notice that, but you actually still have the same problem. With the TalonFX, the sensor is always in phase, so a negative output will make the encoder value decrease, even though your robot will drive forwards. Ideally, you would use setInverted to solve this, so you can give a positive input to go forwards. However, since you use DifferentialDrive, this solution wouldn’t work. Instead, you can negate your right encoder value to make it in phase. In the future, I would recommend not using DifferentialDrive (at least until this behavior is changed).
If you look in the encoders class, the values from the right side are being negated when they are being used to update the driveOdometry in the periodic method in drive, that being said, I did notice that the speed for the right side is not being negated when the speeds are fed into the wheelSpeeds method that the RamseteCommand gets
Whoops, I did miss that. Yeah, your wheel speeds might be the issue. We had a similar issue where the controller would send output towards zero because the speeds were too high because of a unit error, so it makes sense that if the speeds are reversed it would try to correct by reversing that side.
I might be completely wrong here but I wonder if its an issue with your sensor feedback. I notice that in your Encoders class you’re averaging the sensor values read from each side falcon, my thought process is that if once in a while one of the encoders fails to read in time it might throw off the sensor values and screw up the control loop. Try either logging over the course of a path what the voltage output goes to, what the commanded velocity setpoint is and what the actual encoder speeds are, you may have to write your own Command using he Ramsete Controller in order to do that. It also might help just creating a command to run your drivetrain at a setpoint speed and see how well your feedforward and PID is tuned. If your constants are too high it could be causing that issue.
On a completely different train of thought we at one point had a really odd path following problem where our robot would drive forwards fine then just made seemingly random motions and that was due to the fact that our path in pathweaver actually had a hidden S-Curve in it because the points weren’t perfectly aligned and the tangent handles were way too far out. I don’t think that describes your issue but you might want to double check your paths anyway, try manually typing in the point coordinates too.
I have also noticed an error consistently coming from the Talons which may be causing an issue:
ERROR -3 CTR: CAN frame not received/too-stale. Talon FX 6 GetIntegratedSensor
ERROR -3 CTR: CAN frame not received/too-stale. Talon FX 6 GetIntegratedSensorPosition
ERROR -3 CTR: CAN frame not received/too-stale. Talon FX 8 GetIntegratedSensor
ERROR -3 CTR: CAN frame not received/too-stale. Talon FX 8 GetIntegratedSensorPosition
It should be noted that this error is not limited to these two talons as I’ve seen it occur on most if not all of the talons in our system. These don’t come up every time I try to run the path, but they occur often enough to possibly draw some concern
Thinking back on it I actually doubt that the CAN frame too stale would cause the type of error you’re describing. We run a 4 falcon drivetrain and see those errors every so often yet it doesn’t mess with our pathfinding. To rule it out maybe try turning the drive.getEncoders()::getWheelSpeeds call into a lambda that gets the wheel speeds and prints its value just to see if your are getting values that are off.
Ignore what I’m saying there I see you’ve got that dashboard method in all your systems. In shuffleboard you can graph that to see what your values are and see if there’s any weirdness. Also if you’re having loop overruns that could be messing up the PID but thats a tricky thing to fix.
The top left plot is the current x position from the Pose2D fed into the controller, the top right is the current velocity being fed in from the trajectory generated from the path, and the bottom plot is the calculated velocity that the RamseteCommand uses, as seen on line 161-162 in the RamseteCommand class:
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
Another thing to note, when the current x position fount from the encoders is plotted next to the position setpoint from the generated trajectory, there is a bit of a difference
These are all the values used in the calculation of the output velocity.
The current values are read from the sensors, the reference values are read from the generated trajectory, the error values are from comparing the two, the calculated values are the output, and k is used in the calculations for those output values.
The most obvious problem is that your wheel speeds are likely about 3 times higher than what the ramsete controller thinks they are (notice how much the calculated ramsete output has shrunk from the reference trajectory state - this is the controller attempting to compensate for the mismatch). Check your unit scaling.
Ok that’s a good point, the TalonFXs return velocity in units per 100ms, to convert this to meters per second I multiplied the outputted velocities by the calculated distance the robot moves in meters per encoder tick (based on the 2048 ticks on the encoders on the TalonFXs) divided by ten to go from 100ms to seconds:
You have to multiply by 10, not divide by it. Additionally, your encoder code is way too complicated; don’t bother averaging and don’t use sensorcollection (it operates on a slow CAN frame).
Thanks for your help, switching to using the sensorCollections to just going off the talons fixed it, no more clicking! The scale on the current velocity graph is still smaller than the reference velocity by a factor of 5, but it appears to be going the correct speed, so I think its just something about the values I’m putting on the dashboard. Anyway, that switch worked, so thanks.
