Maybe it needs some derivative factor? But I think SimpleFOC’s handling of the derivative needs improvement. I’ve always had trouble whenever I added even a tiny bit of D, and comparing SimpleFOC’s PID to this article I bookmarked a long time ago, I think I know why Improving the Beginner’s PID – Introduction | Project Blog
It’s the “derivative kick”, a spike in the output whenever you change the setpoint (i.e. constantly in angle mode). The solution is to subtract D*(input - lastInput) instead of adding D*(setpoint - input). Unfortunately not easy to fix, since SimpleFOC’s PID takes the error as its argument rather than the new input and calculating the error internally.