The SONARs each get a 10micro sec trigger. This creates an ultrasound PING. I use PULSIN plus PAUSE 10 to input the response and wait for the ringing to stop. Total time for two SONARs = at least 20msec.

To make room for the PWM pulses I could shorten the length of the SONAR PAUSEs. Trying to fit 20msec interrupts clearly might be a problem.

I could trigger the SONARs less often (interrupt driven). I do not need to be measuring the distances so often. The PIC could, at that time, skip a PMW interrupt - or delay it a couple of milliseconds.

Other than that all the code does is fly through various IF THEN state machines depending on the most SONAR readings. That is very simple.

However-----\

If I could calculate closing rates accurately and if I had a finely tuned proportional control system then I might be able to implement a real PID system. Not sure. I do not know how to analyze this.

Ken