I am trying to control a single servo using output compare and a couple of timers. Timer 2 provides the 20ms period and it controls output compare 1 (OC1). OC1 is running in continuous pulse mode, where it uses Timer 2 to count from 0 to anywhere from 0.8-2.2ms. In my code, I have a function called servo_pos() that calculates the needed value for this time, which is placed in the OC1's OC1RS register.
I am able to set the servo position to whatever angle I want at initialization. Currently at initialization, the servo is set to rotate to the neutral 90 degree position and it stays there. However, I want to be able to change position when I tell it to and my servo won't let me do that. For example, I am using a different timer, Timer 4, to create a 2 second delay. Each time Timer 4 reaches its period count, an ISR is called that is supposed to change the servo position using my servo_pos() function. I know the ISR works because I have an LED blinking every 2 seconds. When the OC1RS register is written a new value, my servo seems to try to move to the new position but it seems as if it is fighting to stay at the neutral position (or whatever position I assign it at initialization). My code is given below, if anyone could please try to help me figure out my problem. I've never used a servo before.
#include <p33EV256GM102.h>
#include <xc.h>
#include <stdio.h>
#pragma config ICS = PGD3
#pragma config FWDTEN = OFF
//Fosc=7.37MHz
//Fcy=Fosc/2=3.685MHz (No changes to PLL)
#define Fcy 3685000
void __attribute__((__interrupt__, __auto_psv__)) _T4Interrupt(void);
void servo_pos(int stop_time);
int pos = 1500;
int main(void)
{
TRISBbits.TRISB6 = 0; //LED output
RPOR0bits.RP35R = 0b010000; //remap pin rp35 as OC1
ANSELBbits.ANSB3 = 0; //sets RB3 to digital
TRISBbits.TRISB3 = 0; //RB3 output for OC1
// set servo to neutral position at startup
OC1R = 0; // pulse start time
servo_pos(pos); // move servo to neutral position
OC1CON1bits.OCM = 0b101; // continuous pulse mode
// Configure Timer 2 (default timer for output compare)
PR2 = 9213; // Timer 2 period (20ms) Fcy x 20ms / 8
T2CONbits.TCKPS = 0b01; // Timer 2 prescaler 1:8
T2CONbits.TON = 1; // Enable Timer 2
// Configure Timer 4 for controlling how long the servo performs each action
IPC6bits.T4IP = 1;
IFS1bits.T4IF = 0;
IEC1bits.T4IE = 1;
PR4 = 28789; // Timer 4 period (2s) Fcy x 2s / 256
T4CONbits.TCKPS = 0b11; // Timer 4 prescaler 1:256
T4CONbits.TON = 1; // Enable Timer 4
while(1)
{
}
return 0;
}
void servo_pos(int stop_time) //stop time in microseconds
{
// Configure Output Compare channel 1 (OC1)
OC1RS = ((float)stop_time/1000000.0)*Fcy/8; // pulse stop time (1.5ms) OC1RS = Fcy x 1.5ms / 8
}
void __attribute__((__interrupt__, __auto_psv__)) _T4Interrupt(void)
{
// Clear Timer 4 interrupt flag
IFS1bits.T4IF = 0;
// Toggle LED on RD1
LATBbits.LATB6 = ~LATBbits.LATB6;
servo_pos(pos += 50); //every time ISR is called, change the servo position
}
