PIC32MX: Servo Control

You can read about RC Servo Theory.

Both of these codes can be used with 5 RC servos. The 20ms period is split into 5 x 4ms sections, 1 for each servo.

Servo Control with PWM

This example uses the PWM to control 5 RC servos. It only uses 1 timer.

You can download RCservoSoftPWM.c for the PIC32 here.

/********************************************************************
RCServoSoftPWM.c
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 the output compare pins OC1 - OC5. It divides the 20ms into 5 x 4ms.
A 4ms Timer is set up with Timer 2. The interrupt determines which pin to turn on
at the given duty cycle. The other PWMs are set to zero duty cycle (off). 

Andrew Long
Northwestern University
2-2010

********************************************************************/

/** INCLUDES *******************************************************/
#include <plib.h>
#include "HardwareProfile.h" 

#define MIN_SERVO_DUTY       3000
#define MAX_SERVO_DUTY      25000

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

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

   INTEnableSystemMultiVectoredInt();
   
   // init the output compare modules
   // comment out the servos you do not want to use
   OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
   OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
   OpenOC3( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
   OpenOC4( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
   OpenOC5( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
   
   // init Timer2 mode and period (PR2)
   // Fpb = SYS_FREQ = 80Mhz (From configuration in bootloader code)
   // Timer Prescale = 8
   // PR2 = 0x9C3F = 39,999
   // interrupts every 4 ms
   // 4 ms = (PR2 + 1) * TMR Prescale / Fpb = (39999 + 1) * 8 / 80000000
   CloseTimer2();
   OpenTimer2( T2_ON | T2_PS_1_8 | T2_SOURCE_INT, 0x9C3F);
   ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_7);
   
   mT2SetIntPriority( 7);    // set Timer2 Interrupt Priority
   mT2ClearIntFlag();       // clear interrupt flag
   mT2IntEnable(1);      // enable timer2 interrupts

   int counter;
   while(1)
   {
      // This code ramps up each servo then starts over
      for (RCservo[0] = MIN_SERVO_DUTY; RCservo[0] < MAX_SERVO_DUTY; RCservo[0]+=1)
      {
         int i;
         for (i = 1; i < 5; i++)
         {
            RCservo[i] = RCservo[i-1] + 200;
            if (RCservo[i] > MAX_SERVO_DUTY) RCservo[i] -= MAX_SERVO_DUTY - MIN_SERVO_DUTY;
            
            //Put in short pause
            counter = 500;
            while(counter-- != 0);
         }
      }
   
   }
   CloseTimer2();
   CloseOC1();
   CloseOC2();
   CloseOC3();
   CloseOC4();
   CloseOC5();

} //end main

void __ISR( _TIMER_2_VECTOR, ipl7) T2Interrupt( void)
{
   if (++RCcount >= 5) RCcount = 0;    // 20mS cycle --> 5 interrupts
   
   // Set all PWM pins low  -- comment out the pins you do not want
   SetDCOC1PWM(0);
   SetDCOC2PWM(0);
   SetDCOC3PWM(0);
   SetDCOC4PWM(0);
   SetDCOC5PWM(0);

   // Determine selected servo and set PWM   
   switch(RCcount)
   {
      case 0:            // comment out the pins you do not want
         SetDCOC1PWM(RCservo[RCcount]);
         break;
      case 1:
         SetDCOC2PWM(RCservo[RCcount]);
         break;
      case 2:
         SetDCOC3PWM(RCservo[RCcount]);
         break;
      case 3:
         SetDCOC4PWM(RCservo[RCcount]);
         break;
      case 4:
         SetDCOC5PWM(RCservo[RCcount]);
         break;
   }
   
   // clear interrupt flag and exit
   mT2ClearIntFlag();
} // T2 Interrupt

Servo Control with Digital Output

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.

You can download RCservoSoft.c for the PIC32 here.

/********************************************************************
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();
}