Servo control with a PIC24F via Output Compare module. Operates in CPU idle mode.

Code:

• Graham Keggi shares this code:

  /*************************************************************************/
   /* Program: ServoPWM.c                                                   */
   /* Author: J. Graham Keggi on 6/19/2009                                  */
   /*                                                                       */
   /* Description: Written in MPLAB C30, and tested on a PIC24FJ16GA002 but */
   /* should work for any generic PIC24Fxxxxx. This program contains two    */
   /* functions, ServoInit() and SetServo() to demonstrate the setup and    */
   /* use of the Output Compare module to accurately control a servo.       */
   /*                                                                       */
   /*************************************************************************/
  
  #include "p24FJ16GA002.h"	// Or any PIC24Fxxxxxx.h
  
  // Declare Prototypes
  void ServoInit(void);
  void SetServo(unsigned int position);
  
  // Config for Primary Osc, 20Mhz, HS, No CP, No JTAG, No WDT
  _CONFIG2(IESO_OFF & SOSCSEL_LPSOSC & WUTSEL_LEG & FNOSC_PRI & FCKSM_CSECMD & OSCIOFNC_ON & IOL1WAY_OFF & I2C1SEL_PRI & POSCMOD_HS);
  _CONFIG1(JTAGEN_OFF & GCP_OFF & GWRP_OFF & BKBUG_OFF & COE_OFF & ICS_PGx1 & FWDTEN_OFF & WINDIS_OFF & FWPSA_PR128 & WDTPS_PS32768);
  
  int main(void) {
  	//Set up oscillator
  	OSCCON = 0x2200; // External Oscillator
  	CLKDIV = 0x0000; // Don't divide the frequency
  
  	RPOR3bits.RP6R = 22; // Tie Output Compare 5 to physical pin RP6
  
  	ServoInit(); // Setup the servo module
  
  	SetServo(127); // Set the position, 0-farthest cw, 127-centered, 255-farthest ccw
  
  	Idle(); // Enter low power mode, stops code execution
  }
  
  void ServoInit(void) {
  	OC5CONbits.OCM = 0;    // Turn output compare off so we can modify it
  	OC5R = 0x0000;         // Load primary register with zero duty cycle
  	OC5RS = 0x0000;        // Load secondary register with zero duty cycle
  	OC5CONbits.OCSIDL = 0; // Allow the servo to remain on when CPU is in idle mode
  	OC5CONbits.OCTSEL = 0; // Set Timer2 as clock source
  	OC5CONbits.OCM = 0x6;  // PWM mode on OC, Fault pin disabled
  	PR2 = 0x61A7;          // Load Timer2 PR2 with 0x61A7 = 0d24999 as PWM cycle of 50Hz
  	IFS0bits.T2IF = 0;     // Clear interrupt flag
  	IEC0bits.T2IE = 0;     // Disable interrupts
  	T2CONbits.T32 = 0;     // 2 16-bit timers, allows use of Timer 3
  	T2CONbits.TCKPS = 1;   // Prescale 1:8 for 20Mhz clock frequency
  	T2CONbits.TON = 1;     // Start Timer2
  }
  
  void SetServo(unsigned int position) {
  	position = (float) position * 7.5; // Scale 0-255 input to 624-3200 for full servo range
  	position += 624;                   // Scale is adjustable from 0.5ms-2.5ms to servo specs (24999 = 20ms)
  	OC5R = position;                   // Write primary register
  	OC5RS = position;                  // Write secondary register
  }