I am assuming you are trying to turn to a specific gyro angle here.
I am curious why you wanting to give it a minimum speed. It seems like that would probably make it way more likely to overshoot.
I am not too familiar with this method (assuming you are using the wpilib DifferentialDrive class) but I feel like you want your first parameter (xSpeed) to be zero and your second parameter (zRotation) to be nonzero.
I don’t think there is a clearly defined acceleration parameter you would want here so I think you should omit that argument.
This treats the output of the gyro PID as a velocity and then gives the velocity to the feedforward which then converts that velocity into a voltage. I divided by 12 because the drive method you are using wants the inputs between -1 and 1, and the voltage goes up to around 12. With this approach, you shouldn’t need the if statements because it should turn to the right angle regardless of where the final angle is.
You’ll want to characterize your drivetrain with SysID to get the feedforward constants to put in the constructor.
This thread talks more about combining feedforward and PID and you might find it helpful for trying to understand what is going on.
Disclaimer: lots of people know way more about this than me so they may have better advice
I am looking at your github and seeing lots of weird things but I don’t know where exactly the error is coming from. What command are you using to turn to an angle, and where are you calling it from? And where are you setting what angle to turn to?
And to answer your question from the other thread you linked, SysID gives you the values that you pass into the SimpleMotorFeedforward constructor, namely kV, kS, and kA. The velocity argument that you pass into the feedforward.calculate() method must change depending on the error from the target, otherwise you would just move at a constant velocity the whole time. The feedforward method converts a desired velocity into a corresponding voltage to send to your motors.
Using the SimpleMotorFeedforward class is more complicated than just setting a minimum motor velocity to overcome static friction. Although I don’t think static friction is the problem, the class with fix that for you too with the kS term, so you will not have to explicitly put that in your code like you trying to
Basically it treats the output of the positional PID loop as a velocity, although I’m not sure if it has meaningful units. For example, when the position error is zero (meaning you are at the desired angle), you get a zero velocity output, which is good because if you are already at the correct angle then you don’t wanna move any more. When the position error is bigger (like 90 degrees away from the desired angle), you get a larger output of the PID which is just a larger velocity. You need a larger velocity in this case because you are farther away. As this larger velocity takes you towards the desired angle, the output of the PID should decrease (meaning velocity will decrease) and you will slowly come to a stop at the desired angle, if everything is tuned right.
The velocity output from the PID goes into the feedforward.calculate() method which converts it into a voltage to send to your motors.
Looking at the other thread though, I have a feeling it still won’t work even after you put this line in. So could you answer these questions?
I thought so too although when I pass the parameters in as they are supposed to work the speed and rotation are switched so it rotates when it should drive straight and drives straight when it should rotate