PIC32MX: Servo Control

You can read about RC Servo Theory.

Digital Output Example

This example is based on the code from PIC18. It uses two timers and up to 5 digital outputs. The PWM pins can be saved for other motor controls.

/********************************************************************
RCServoSoft.c a.long 2009-09-11
This routine is for generating the code signals used to control rotor position
for up to five little RC servos. An RC servo signal repeats every 20ms:
High 300 to 2500 uS to indicate rotor position, and then Low. This sample code
uses pins RD0 - RD4 but you can switch it to whichever pins (and as few) as you wish.
This routine uses Timer 3 to generate the RC servo signals in semi-software.
Timer2 and Timer3 can be used for hardware PWM, so you may be interested in changing
the timer.

We'll keep a 1KHz interrupt routine going with Timer2. This ISR sets all outputs
low every 4mS. Using a Timer 3 ISR, we turn one of the five outputs high,
every 300-2500 uS prior to the next 4mS boundary. If you have other stuff in the
1mS ISR, put it after this RC servo code, so as not to disturb our timing.
This code has been generated with reference to RCservoSoft.c from m.peshkin.

********************************************************************/
 
/** INCLUDES *******************************************************/
#include "HardwareProfile.h"

#define LOW                  0
#define HIGH               1

// Define PIN_DX for output
#define PIN_D0               LATDbits.LATD0
#define PIN_D1               LATDbits.LATD1
#define PIN_D2               LATDbits.LATD2
#define PIN_D3               LATDbits.LATD3
#define PIN_D4               LATDbits.LATD4

int RCservo[5]; // Desired high durations here 375-3125 values give 0.3 mS
            // to 2.5 mS.
int RCcount;
int OneMStimer;

int main(void)
{
   int i;

   // Configure the proper PB frequency and the number of wait states
   SYSTEMConfigPerformance(SYS_FREQ);

   INTEnableSystemMultiVectoredInt();

   // Set PINS D0 - D4 as digital outputs
   LATD |= 0x001F; TRISD &= 0xFFE0;

   // init Timer2 mode and period (PR2)
   // Fpb = SYS_FREQ = 80Mhz (From configuration in bootloader code)
   // Timer Prescale = 2
   // PR2 = 0x9C3F = 39,999
   // interrupts every 1 ms
   // 1 ms = (PR2 + 1) * TMR Prescale / Fpb = (39999 + 1) * 2 / 80000000
   CloseTimer2();
   OpenTimer2( T2_ON | T2_PS_1_2 | T2_SOURCE_INT, 0x9C3F);
   ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_5);

   // init Timer3 mode and period (PR3)
   // Fpb = SYS_FREQ = 80Mhz (From configuration in bootloader code)
   // Timer Prescale = 64
   // PR3 = 0xFFFF = 65535
   // Ticks every 0.8 uS = TMR Prescale / Fpb = 64 / 80000000
   CloseTimer3();
   OpenTimer3( T3_ON | T3_PS_1_64 | T3_SOURCE_INT, 0xFFFF);
   ConfigIntTimer3(T3_INT_ON | T3_INT_PRIOR_6);

   while(1) // demo routine moves all five through their range of duty cycles
   {
      for (RCservo[0] = 375; RCservo[0] < 3125; RCservo[0]+=1)
      {
         for (i = 1; i < 5; i++)
         {
            RCservo[i] = RCservo[i-1] + 200;
            if (RCservo[i] > 3125) RCservo[i] -= 2750;
         }
         OneMStimer = 1;
         while(OneMStimer); // sweep slowly through available duty cycles
                       // uses the 1 ms interrupt of Timer 2
      }
   }
   CloseTimer2();
   CloseTimer3();
} //end main


void __ISR(_TIMER_2_VECTOR,ipl5)Timer2Handler(void)
{
   if (++RCcount >= 20) RCcount = 0;    // 20mS cycle --> 20 interrupts 

   if ((RCcount & 3) == 0) // on the 4mS boundaries turn all the pins low
   {
      PIN_D0 = LOW;
      PIN_D1 = LOW;
      PIN_D2 = LOW;
      PIN_D3 = LOW;
      PIN_D4 = LOW;

      WriteTimer3(60536 + RCservo[RCcount>>2]);    // yes 60536, not 65536.
                          // Go high 4000uS (5000 * 0.8uS) from now,
                        // and sooner due to desired High period
   }

   // your ISR stuff goes here, after the RC part, so as not to disrupt the timing
   --OneMStimer;
   // clear interrupt flag and exit
   mT2ClearIntFlag();
}

void __ISR(_TIMER_3_VECTOR, ipl6)Timer3Handler(void) // this ISR is called when Timer 3
                                        // times out, to set one of the RC
                                       // servo output pins high
{
   switch(RCcount>>2)
   {
      case 0:                // comment out the pins you do not want
         PIN_D0 = HIGH;
         break;
      case 1:
         PIN_D1 = HIGH;
         break;
      case 2:
         PIN_D2 = HIGH;
         break;
      case 3:
         PIN_D3 = HIGH;
         break;
      case 4:
         PIN_D4 = HIGH;
         break;
   }
   // clear interrupt flag and exit
   mT3ClearIntFlag();
}

You can download RCservoSoft.c for the PIC32 here.