We have a pathplanner path that simply moves forward by 3 meters. The path should be taking around 2 seconds to complete. However, it moves about 0.5 meters and the stops moving. We have tried changing the PID constants, and we have increased kP to a large amount, but the path continues to move an extremely small length. The telemetry graphs in pathplanner show that the velocity given to the motors was 0 the entire time of running the path even though the robot did move a little bit.
It would help a lot to know which path you’re attempting to follow. I’m not sure exactly what is causing your problem, but I have a few notes on your code. Hopefully something in here will help you.
Your deploy directory is a terrible mess with files at the wrong level and extraneous nested directories.
Your module locations seemed to be mixed up, with left and right swapped. I also see that DriveTrain::getPose is inverting the rotation, which I suspect is related.
I don’t recommend accessing buttons directly inside a subsystem.
I’m not sure why the drive subsystem is operating the climber.
DriveTrain::Periodic should be using a for loop over the modules. I don’t see any mistakes but copy-and-paste code like that is hard to maintain and easy to introduce bugs in. Better, this code should be in a SetState method on SwerveModule. Is that what SwerveModule.SetVelocity is intended to be?
The same code is comparing floating point numbers to zero, which is always a bad idea. Should be using something like IsNear.
DriveTrain::getRobotRelativeSpeeds appears to be returning the commanded module state. It ought to be reporting the actual module state instead. SwerveModule ought to have a GetState method (similar to GetPosition).
Also, I think the state that getRobotRelativeSpeeds returns is never even changed at all, which is why they are 0. The update happens in BaseDrive which I don’t see being called in auto. But yes even if BaseDrive was called, it would be the commanded states rather than the actual states (which it shouldnt be).
