Well working FOC

Finally I found the culprit of the misbehaving current regulators: they need very high integral gain!
This version removes all the kludges added to circumvent this fact and it sets the default of curki to 20000. If you're upgrading from an earlier version make sure to set curki to 20000.