Pic Firmware

Changing the servo frequency involves a few code changes.
I will go through the changes needed below, they are not too obvious.

You are correct, the ultrasonic sensors are all triggered at a regular interval.
There is no command to disable any of the connectors.

The triggering and reading of the ultrasonic sensors is all handled by the ReadAllUltrasonics function in
main.c.

Changing the servo frequency

The servo output is generated from a clock signal output by the PIC.
This signal is linked to the internal clock.
To change it we need to alter a few places in the code.

The main change is to the ConfigureOscillator function in system.c.
We need the clock to be at one of these speeds:

• 8 MHz → 30 Hz servo operation
  

  void ConfigureOscillator(void) {
  	// 8 MHz
  	OSCCONbits.SCS = 0b10;	    // Set the clock to the internal oscillator
      OSCCONbits.IRCF = 0b1110;	// Select the 8 MHz postscaler
      OSCCONbits.SPLLEN = 0;		// Disable the 4x PLL (overriden in CONFIG2 by PLLEN = ON)
  }
  	

• 16 MHz → 60 Hz servo operation
  

  void ConfigureOscillator(void) {
  	// 16 MHz
  	OSCCONbits.SCS = 0b10;	    // Set the clock to the internal oscillator
      OSCCONbits.IRCF = 0b1111;	// Select the 16 MHz postscaler
      OSCCONbits.SPLLEN = 0;		// Disable the 4x PLL (overriden in CONFIG2 by PLLEN = ON)
  }
  	

• 32 MHz → 120 Hz servo operation
  

  void ConfigureOscillator(void) {
  	// 32 MHz
  	OSCCONbits.SCS = 0b10;	    // Set the clock to the internal oscillator
      OSCCONbits.IRCF = 0b1110;	// Select the 8 MHz postscaler
      OSCCONbits.SPLLEN = 1;		// Enable the 4x PLL (overriden in CONFIG2 by PLLEN = ON)
  }
  	

We also change the PLLEN setting in configuration_bits.c.

• 8 or 16 MHz → 30 or 60 Hz servo operation
  #pragma config PLLEN = OFF // PLL Disable (4x PLL disabled)
• 32 MHz → 120 Hz servo operation
  #pragma config PLLEN = ON // PLL Enable (4x PLL enabled)

At this point the servos will operate at the correct speed, however the ultrasonic reading rate may be too fast.
We can fix this by changing the main function in main.c slightly.

First add a counter variable inside main:

void main(void) {
	int usmTriggerCount = 0;

	// Configure the oscillator for the device
	ConfigureOscillator();

Now modify the ultrasonic reading section to increase and check this counter:

        // Get the ultrasonic readings
		if (PIR3bits.TMR4IF) {
			// Clear the interrupt flag
			PIR3bits.TMR4IF = 0;

			++usmTriggerCount;
			if (usmTriggerCount >= 1) {
				// Read all of the ultrasonics and trigger the next set of readings
				ReadAllUltrasonics();
				usmTriggerCount = 0;
			}
		}

Finally you will need to change the counter limit depending on the servo frequency:

• 8 MHz → 30 Hz servo operation
  if (usmTriggerCount >= 1) {
• 16 MHz → 60 Hz servo operation
  if (usmTriggerCount >= 2) {
• 32 MHz → 120 Hz servo operation
  if (usmTriggerCount >= 4) {