The Profiled pid controller needs to be reset before each trajectory in auto. However , l was puzzled when and where to call the reset() method. Based on my understanding, l should reset it like below:
Any help is appreciated!
You would reset the theta angle to the odometry heading, not 0. You can also use the drivetrains ChassisSpeeds calculated from the current module wheel speeds to specify the omega angle velocity if you think that is important.
Yes
Besides, should the other two controllers(xController and yController) be reset?
As long as they only have a P gain, they don’t need to be reset. Past error doesn’t affect the proportional gain.
Thanks .
It seems that the holonomic controller do reset the thetacontroller, so why doesn’t it take effect?
That’s only for the first call to calculate, as you can see by the m_firstRun boolean.
Yeah, if l enable the auto mode again and again ,will the first run be reset to true?
No, because you use the same HolonomicDriveController.
Got it. Besides , l have noticed that the holonomic driver do calculate a field relative speed , however , my field relative mode works well in the teleop, But in the auto , l doesn’t work as l expect. l have checked the my gyro and the value it gets, they are all correct. Can you give me some suggestions about where maybe the error lie？
When l use sequentialcommandgroup to do auto and add a swervecontrollercommand in it, will it update the getpose methond in the swerve controllercommand periodically?
What do you mean? SwerveControllerCommand accepts a Pose2d supplier that it periodically calls during execution to get the robot pose.
Yes , however , the last rotation of pose2d it gets is 10 degree bigger than the odometry’s rotation? It;s strange.
l am puzzled that my profiled PID controller just can’t calculate the right speed of omega. l think that there is something wrong with my profiled PID controller. My question is that there is an error of the Profiled PID Controller, and it do have an output , but my swerve just can’t rotate to eliminate the error. l have double checked my auto drive method , it seems that there is no problem with it. l try to multiply the theta speed calculated by the profiled pid controller with kMaxOmega just like in the teleop. However , the robot still end with an error existed, a really big error. l have also checked the omegaspeed in the end point , it is really big too. But my swerve just don’t move!
Any help is appreciated!
If you fix your coded from a four wheel swerve (which you don’t have) to a two-wheel swerve (your actual setup) you may have better results.
I don’t think it’s the problem of a two wheel swerve since it’s located in the diagonal line. Second,l have deeply tested the whole process of the auto. I find that my final state do have a speed output, but it won’t let the robot to rotate to eliminate the theta controller’s error. That’s strange.
Path planning needs a finely tuned device to work properly. As your robot traverses a path, the path planner calculates specific velocities and positions that it sends to your holonomic drive controller, which sends values to your kinematics controller which is expecting four wheels. Even if you set them on the diagonal, you’re not gonna get as optimum as results as if you were to setup your kinematics with 2 wheels.
Could you send a link to your code? I think I’m facing a similar issue.
