Microchip has got a few appnotes on their sites regarding motor control. However the source codes are in asm or C. Nevertheless it may help you in system analysis and design. If you could give exacting requirement then I can also spend some time on it. PID is bothering me too. I design industrial battery chargers and DC thyristor control. Employing a PID control there would improve the transient response of the system