Control servo mediante la comparación de salida y los temporizadores de dsPIC33

Estoy tratando de controlar un solo servo usando la comparación de salida y un par de temporizadores. El temporizador 2 proporciona el período de 20 ms y controla la comparación de salida 1 (OC1). OC1 se está ejecutando en modo de pulso continuo, donde usa el temporizador 2 para contar de 0 a 0,8-2,2 ms. En mi código, tengo una función llamada servo_pos () que calcula el valor necesario para este tiempo, que se coloca en el registro OC1RS del OC1.

Puedo establecer la posición del servo en el ángulo que desee en la inicialización. Actualmente en la inicialización, el servo está configurado para girar a la posición neutral de 90 grados y permanece allí. Sin embargo, quiero poder cambiar de posición cuando se lo diga y mi servo no me permite hacerlo. Por ejemplo, estoy usando un temporizador diferente, Temporizador 4, para crear un retraso de 2 segundos. Cada vez que el temporizador 4 alcanza su recuento de períodos, se llama a un ISR que se supone que cambia la posición del servo utilizando mi función servo_pos (). Sé que el ISR funciona porque tengo un LED parpadeando cada 2 segundos. Cuando se escribe un nuevo valor en el registro OC1RS, mi servo parece intentar moverse a la nueva posición, pero parece que está luchando por permanecer en la posición neutral (o en cualquier posición que le asigne en la inicialización). Mi código se proporciona a continuación, si alguien puede intentar ayudarme a resolver mi problema. Nunca he usado un servo antes.

#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

}