[dyanoshak]

Now that you've got it spinning, it is just a matter of tuning your constants.

I saw the same behavior when I was playing with a kit encoder, CIM, and a toughbox on our robot. I found that a much lower P with a little bit of I seemed to work well. I was using P = 0.3 and I = 0.001. It just takes time to find the perfect values for your system.

Good luck with tuning!

-David