Servo motor interface with PIC

[Udayan Sinha]

I am trying to create a servo motor control library for PIC to control up to 4 servos (standard 180 deg rotation types). I wrote the code for sending the pulse (1-2ms) according to the angle entered and tested it out. The problem is that the servo arm is oscillating (for eg. if I enter 90 deg, the servo keeps moving back and forth all the way from 0 deg to 90 deg).
I am using two 16-bit timers(1 for the servo pulse and the other for timing the 20ms delay). I am using a 20Mhz crystal for the PIC.  I checked the pulse timing using the PICkit-2 logic tool and it seems perfectly fine so I can't figure out where I have gone wrong. Please help me out.

My code:

//timer values for servo angle control
union servo1
{
    unsigned char srv1[2];
    unsigned int srv1_16;
}servo_1;

union servo2
{
    unsigned char srv2[2];
    unsigned int srv2_16;
}servo_2;

union servo3
{
    unsigned char srv3[2];
    unsigned int srv3_16;
}servo_3;

union servo4
{
    unsigned char srv4[2];
    unsigned int srv4_16;
}servo_4;

void timer_init(void);
void servo_init(unsigned char number_of_servos);
void interrupt_init(void);
void interrupt isr_h(void);
void interrupt low_priority isr_l(void);
void timer0_isr(void);
void timer3_isr(void);
void servo_move(unsigned char angle, unsigned char servo_number);

unsigned char iteration_count=0, no_of_servos=1;
void main()
{
    servo_init(no_of_servos);    //initialize servo control
    interrupt_init();            //initialize interrupts
    servo_move(180, 1);            //rotate servo motor 1 by 90deg
    while(1);
}
//servo motor control initialize function
void servo_init(unsigned char number_of_servos)
{
    timer_init();         //initialize timer 0
    //PORTD used for servo motor control
    switch(number_of_servos)
    {
        case 1:
            TRISD=0xFE;        //1 servo motor is used
            break;

        case 2:
            TRISD=0xFC;        //2 servos motors are used
            break;

        case 3:
            TRISD=0xF8;        //3 servos motors are used
            break;

        case 4:
            TRISD=0xF0;        //4 servos motors are used
            break;
    }
}
//servo motor control function
void servo_move(unsigned char angle, unsigned char servo_number)
{
    switch(servo_number)
    {
        case 1:
            servo_1.srv1_16=60536-(27.77*angle);
            break;

        case 2:
            servo_2.srv2_16=60536-(27.77*angle);
            break;

        case 3:
            servo_3.srv3_16=60536-(27.77*angle);
            break;

        case 4:
            servo_4.srv4_16=60536-(27.77*angle);
            break;
    }
    //start timer
    TMR3ON=1;
    TMR0ON=1;
}
//timer initialize function
void timer_init(void)
{
    //timer 0
    T08BIT=0;          //timer used in 16-bit mode
    T0CS=0;            //use as timer
    PSA=1;             //prescaler not used
    //initialize timer 0 registers
    TMR0H=0xFF;
    TMR0L=0xFF;
    TMR0ON=0;           //timer 0 is off for now
    //timer 3
    T3CON|=0x80;           //RD16=1; timer registers accessed together
    //1:8 prescaler
    T3CKPS0=1;
    T3CKPS1=1;
    TMR3CS=0;         //use as timer
    TMR3ON=0;        //timer 3 is off for now
    //initialize timer 3 registers
    TMR3H=0xCF;
    TMR3L=0x2C;
    //initialize servo angle values to 0deg
    servo_1.srv1_16=0xFFFF;
    servo_2.srv2_16=0xFFFF;
    servo_3.srv3_16=0xFFFF;
    servo_4.srv4_16=0xFFFF;
}
//interrupt initialize function
void interrupt_init(void)
{
    IPEN=1;             //enable interrupt priority
    GIEH=1;             //enable high priority interrupts
    GIEL=1;             //enable low priority interrupts
    TMR0IF=0;             //clear interrupt flag
    TMR0IE=1;             //enable timer 0 interrupt
    TMR0IP=0;             //low priority interrupt on timer 0
    TMR3IF=0;             //clear interrupt flag
    TMR3IE=1;             //enable timer 3 interrupt
    TMR3IP=1;             //high priority interrupt on timer 3
}
//high priority ISR function
void interrupt isr_h(void)
{
    asm("CALL _timer3_isr");
}
//low priority ISR function
void interrupt low_priority isr_l(void)
{
    asm("CALL _timer0_isr");
}
//timer 0 ISR
void timer0_isr(void)
{
    TMR0ON=0;           //turn off timer
    TMR0IF=0;           //clear interrupt flag
    if(iteration_count<no_of_servos)              //signals need to be sent to servo motors
    {
        switch(iteration_count)
        {
            case 0:
                TMR0H=servo_1.srv1[1];
                TMR0L=servo_1.srv1[0];
                iteration_count++;
                RD0=1;
                break;

            case 1:
                RD0=0;
                TMR0H=servo_2.srv2[1];
                TMR0L=servo_2.srv2[0];
                iteration_count++;
                RD1=1;
                break;

            case 2:
                RD1=0;
                TMR0H=servo_3.srv3[1];
                TMR0L=servo_3.srv3[0];
                iteration_count++;
                RD2=1;
                break;

            case 3:
                RD2=0;
                TMR0H=servo_4.srv4[1];
                TMR0L=servo_4.srv4[0];
                iteration_count++;
                RD3=1;
                break;
        }
        TMR0ON=1;             //start timer again
    }
    else                                                     //all servos motors have been serviced
    {
        switch(no_of_servos)
        {
            case 1:
                RD0=0;
                break;

            case 2:
                RD1=0;
                break;

            case 3:
                RD2=0;
                break;

            case 4:
                RD3=0;
                break;
        }
    }
}
//timer 3 ISR
void timer3_isr(void)
{
    TMR3ON=0;           //turn off timer
    TMR3IF=0;           //clear interrupt flag
    //restart process
    iteration_count=0;       
    //reload timer registers
    TMR3H=0xCF;
    TMR3L=0x2C;
    TMR0H=0xFF;
    TMR0L=0xFF;
    //turn on timers
    TMR3ON=1;
    TMR0ON=1;
}

Please let me know if you need any other details.

[weky]

Hi! Can you tell me how much time is between two pulses? I was working on some project with RC servos so I can help.... But it's too late for me to go thru code now....

[Udayan Sinha]

Hi! Can you tell me how much time is between two pulses? I was working on some project with RC servos so I can help.... But it's too late for me to go thru code now....

Pulses must be sent every 20ms. Please do let me know if you get a working code.

[dannyf]

it seems perfectly fine

What does that mean?

Please do let me know if you get a working code.

The issue is fairly simple: articulate what you expect (why you think your code will produce the expected results) and show what you actually got.

[Udayan Sinha]

The issue is fairly simple: articulate what you expect (why you think your code will produce the expected results) and show what you actually got.

Already done that. Read description carefully.

[Udayan Sinha]

Got it working guys 
Guess what the problem was: a faulty connection on a crusty breadboard

[dannyf]

Here is my implementation - a multi-channel, synchronized pwm generator using just 1 timer (timer0 in this case).

In this particular case, I implemented four independent pwm channels (PB0, 1, 7 and 6), over a 20ms period.

It can be easily expanded to generate additional channels (or fewer if you want). Very little overhead - the glitches in PB4 are due to when the timer isr is being serviced - 20us in the pro mode and slightly higher in the free mode.

Very handy for low-end pics that lack a hardware pwm module.

Invoking it is easy:

Code: [Select]

pwm_init(TMR0_PS_2x, 160); //reset the pwm period, roughly 2 * 160 * 256 * 1/4Mhz=20ms
pwm0_setdc(1<<0, 6); //pwm ch0, on pin P0, dc=6/160
pwm1_setdc(1<<1, 7); //pwm ch1, on pin P1, dc=7/160
pwm2_setdc(1<<7, 8); //pwm ch2, on pin P7, dc=8/160
pwm3_setdc(1<<6, 18); //pwm ch3, on pin P6, dc=18/160

After that, everything is done in the background and fully transparent to the user.