PWM control RC servo motor

[king_diaw]

hello everyone, i'm just now finished programming to control servo motor (CS40S)
My project is running well, I'm can control the movement(0~180 degree) of servo motor by setting the duty cycle.
In this project I'm using PIC18F2550 with 20MHz oscillator.
 

Code:

//include the library
#include "p18f2550.h"
#include "delays.h"

#pragma config WDT=OFF
#pragma config LVP=OFF
#pragma config DEBUG=OFF
#pragma config PWRT=OFF

//define IO that will use in the Programming

#define sw1 PORTAbits.RA0
#define sw2 PORTAbits.RA1
#define sw3 PORTAbits.RA2
#define srm1 PORTAbits.RA3
#define srm2 PORTAbits.RA4
#define srm3 PORTAbits.RA5
#define srm4 PORTCbits.RC0
#define srm5 PORTCbits.RC1
#define srm6 PORTCbits.RC2
#define srm7 PORTCbits.RC4
#define srm8 PORTCbits.RC5
#define srm9 PORTCbits.RC6
#define srm10 PORTCbits.RC7
#define srm11 PORTBbits.RB0
#define srm12 PORTBbits.RB1
#define srm13 PORTBbits.RB2
#define srm14 PORTBbits.RB3
#define srm15 PORTBbits.RB4
#define srm16 PORTBbits.RB5
#define led PORTBbits.RB7

//Prototype function
void init_io(void);
void init_interrupt(void);
void blink(void);
void delay(unsigned long i);
void prog1(void);
void prog2(void);
void prog3(void);
void servo(unsigned char h);

//main function

void main()
{
unsigned char i2c_data;
init_io();
blink();
while(1)
{
if(sw1==0)   //sw1 is active low
servo(0);
else if(sw2==0)  //sw2 is active low
servo(1);
else if(sw3==0)  //sw3 is active low
servo(2);
}
while(1);
}

void init_io()
{
ADCON1=0x0F; //Configure ADC0:12 as Digital IO
TRISA=0b00000111;
TRISB=0X00;
TRISC=0X00;
PORTA=0;
PORTB=0;
PORTC=0;
}

void blink()
{
unsigned char i;
for(i=0;i<5;i++)
{
led=~led;
Delay10KTCYx(35);
}
led=1;
}
void servo (unsigned char h)
{
unsigned char i;
if(h==1) //forward
{
for(i=0;i<100;i++)
{
PORTA=0xff;
Delay100TCYx(30);//113 180degree~225us
Delay10TCYx(28);
PORTA=0;
Delay100TCYx(125);
}
blink();
}
else if(h==2)
{
for(i=0;i<100;i++)
{
PORTA=0xff;
Delay100TCYx(25);//25 90degree~500us
PORTA=0;
Delay100TCYx(125);
}
blink();
}
else if(h==0) //origin
{
for(i=0;i<100;i++)
{
PORTA=0xff;
Delay100TCYx(40);  //40 origin~800us
PORTA=0;
Delay100TCYx(125);
}
blink();
}

}

[Doug]

Actually, I wrote about this on my old blog but it is for 8051. Please see here

http://mcu-programming.blogspot.com/2006/09/servo-motor-control.html