Asynchronmotor mittels Inverter und ATMega 2560 auf Position regeln

Hallo

Ich muss einen Motor mittels Mikrocontroller auf eine Sollposition 
regeln.

Die Istposition ist bekannt und wird laufend aktualisiert. Die 
Ansteuerung erfolgt über die Drehrichtung und die Drehzahl.

Wie lässt sich allgemein am leichtesten eine (PI oder PID) 
Positionsregelung erstellen?


Daten der Anlage:
- Inverter: Lenze i550 mit CANopen
- Mikrocontroller: ATMega2560 + MCP2560
- Motor: Lenze Asynchronmor mit Inkrementalgeber und 
istpositionsauswertung (32bit)
- Sollpositionsdaten werden seriell vom Mikrocontroller eingelesen