

PIC Microcontroller Code Fragments
Clock shown as pin 2 on JP2 is connected to B7 on the PIC and Reset on pin 3 is connected to C1 on the PIC
Global Variables
// Number of servos
// Due to timing limitations you can really only use 9 servos with a 4017
#define SERVOCOUNT 5
// This is 65536 minus number of clock ticks in 1 ms plus or minus a fudge factor
// This was determined using an oscilloscope
#define SERVOBASE 55067
// This union allows access to the servo timing as an integer and as two bytes
// This makes the interrupt code faster and the whole thing more stable
union ServoTimers
{
unsigned int position;
unsigned char byte[2];
} servotimer[SERVOCOUNT];
// This is the 12 bit servo value 0=>1ms 2048=>1.5ms 4096=>2ms
unsigned int servo[SERVOCOUNT];
// Servo trims can be used to align the center position of the servo a small ammount
signed int servotrim[SERVOCOUNT];
// Counter for sending the servo signals sequentially
unsigned char currentservo;
// Used to track the state of the Reset signal
BOOL servoReset;
// Used for pulsing the Clock signal
BOOL servoOn;
Interrupt Code
void HighPriorityISR()
{
// If Timer0 Interrupt is Flagged
if (INTCONbits.TMR0IF) {
// Reset Interrupt Flag
INTCONbits.TMR0IF = 0;
// If the servo has been reset hold C1 low for almost 20ms
if (servoReset) {
servoReset = 0;
TMR0H = 0xF8;
TMR0L = 0xAF;
LATCbits.LATC1 = 0;
// Otherwise bring Reset/C1 high briefly and start clocking
// servo data out via Timer 3
} else {
servoReset = 1;
TMR0H = 0xFF;
TMR0L = 0xF0;
TMR3H = 0xFF;
TMR3L = 0xF0;
currentservo=0;
LATCbits.LATC1 = 1;
}
}
if (PIR2bits.TMR3IF) {
PIR2bits.TMR3IF = 0;
// For each servo pulse set Clock/B7 high momentarily
// then hold B7 low for the current servos desired pulse width
if (currentservo < SERVOCOUNT) {
if (servoOn) {
TMR3H = servotimer[currentservo].byte[1];
TMR3L = servotimer[currentservo].byte[0];
LATBbits.LATB7 = 0;
servoOn = FALSE;
currentservo++;
} else {
TMR3H = 0xFF;
TMR3L = 0xF0;
LATBbits.LATB7 = 1;
servoOn = TRUE;
}
// Otherwise dont call this interrupt for a while
} else {
TMR3H = 0x00;
TMR3L = 0x00;
LATBbits.LATB7 = 0;
servoOn = FALSE;
}
}
}
Servo Control Functions
void ServoInit(void)
{
int i;
servoReset = FALSE;
servoOn = FALSE;
// Center all servos and zero all trims
for (i=0; i<SERVOCOUNT; i++) {
servo[i] = 2048;
servotrim[i]=0;
}
// UpdateServos must be called after servo[i] or servotrim[i] is modified
UpdateServos();
currentservo = 0;
}
void UpdateServos(void) {
int i;
// Convert each 12 bit servo position into interrupt timing
// The maximum time for an interrupt to overflow and trigger
// with a given prescaler and clock is with TMRxH and TMRxL
// set to 0, a timer setting of 0xFFFE will trigger the interrupt instantly
for (i=0; i < SERVOCOUNT; i++) {
servotimer[i].position = SERVOBASE - (3*servo[i]) + servotrim[i];
}
}
This setup allows the servos to be positioned with 12bit resolution.
servo[0] = 0 => 1ms => Left
servo[0] = 2048 => 1.5ms => Center
servo[0] = 4095 => 2ms => Right
The trim control is done independently of the servo position setting and does not effect the positional resolution. The actual resolution will vary between different manufacturers and models and this controller unfortunately does not work with the Draganflyer.
No comments:
Post a Comment