Have you fixed the easier of your 2 issues yet? Expecting it to work correctly at 1220Hz when it is waiting for 50Hz may be a bit of a stretch. You can create a multi-channel, interrupt-driven, software PWM with a PIC16 and a 16-bit timer - such as TIMER1. If you don't know how to do that, then select a PIC18 and use DT's 2-CH Hardware Servo Driver (see CMS articles on the right of the HOME page). It is limited to only 2 channels but will work for your project for now.
When you have that working correctly, then you can progress to the PID stuff which looks like this:
Bookmarks