PIC32MX: Servo Control
From Mech
Revision as of 12:44, 9 February 2010 by Andrew Long (talk | contribs)
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();
}
