This may not be as simple as you'd like, but here is a program I wrote to position servos (with speed control). It accepts commands over the serial port. The compiler is from CCS.
#include <16F873.h>
#fuses HS,NOWDT,NOPROTECT,NOLVP,NOBROWNOUT
#use delay(clock=20000000)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7)
#use i2c(SLAVE, SCL=PIN_C3, SDA=PIN_C4, ADDRESS=0xA0, FAST, FORCE_HW)
#include <stdlib.h>
#include <input.c>
static short pa1_rcvd = FALSE; // First byte of preamble received
static int pdata_byte = 0; // Packet data byte number
static int cmd_channel; // Commanded servo channel number (1-8)
static long cmd_pdur; // Commanded servo pulse duration
static int cmd_rate; // Commanded servo move rate (us per servo pulse - there are 50 pulses / sec)
static long pdur_msb;
#INT_SSP
void ssp_interupt ()
{
int rcv_data;
rcv_data = i2c_read(); // This is the I2C address byte
rcv_data = i2c_read(); // Data byte
if (pdata_byte > 0 ) { // The preamble has been received
switch (pdata_byte) {
case 1:
cmd_channel = rcv_data;
pdata_byte++;
break;
case 2:
pdur_msb = rcv_data;
pdata_byte++;
break;
case 3:
cmd_pdur = (pdur_msb<<8) | rcv_data;
pdata_byte++;
break;
case 4:
cmd_rate = rcv_data;
pdata_byte++;
break;
default:
break;
}
}
if (rcv_data == 0xFF) {
if (pa1_rcvd) // This is the second byte of the preamble
pdata_byte = 1; // Set packet data byte number to 1
else
pa1_rcvd = TRUE; // This may be the first byte of the preamble
}
else
pa1_rcvd = FALSE;
}
void main()
{
#define CH1_SP PIN_C5 // Channel 1 servo pulse output
// Pulse duration limits
#define PD_MAX 2000
#define PD_MIN 1000
long tpdur[9] = {0, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // Target pulse duration
long cpdur[9] = {0, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // Current pulse duration - used for rate function
int rate[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
long t1_preset;
int active_pulse, i, j;
// Float the I2C pins
output_float(PIN_C3);
output_float(PIN_C4);
output_low(CH1_SP);
// Timer 0 is the R/C servo pulse cycle timer
// Set to internal clock (5 MHz) / 256 = 51.2 us per count
// This produces a maximum delay of 13.1 ms, so the timer will have to be used twice per desired 20 ms cycle period.
setup_timer_0(RTCC_INTERNAL | RTCC_DIV_256);
// Timer 1 is the R/C servo pulse duration timer
// Set to internal clock (5 MHz) = 0.2 us per count
setup_timer_1(T1_INTERNAL | T1_DIV_BY_1);
// Enable interrupts
enable_interrupts(INT_TIMER1); // Enable the Timer 1 overflow interrupt
enable_interrupts(INT_SSP);
enable_interrupts(GLOBAL);
// Servo pulse cycle loop - executes once every 20 ms
while (TRUE) {
active_pulse = 1;
// Calculate the current pulse durations if the rate function is used (rate != 0)
for (j=1; j<=8; j++) { // For each servo channel
if (rate[j]>0 && tpdur[j]>cpdur[j]) {
cpdur[j] = cpdur[j] + rate[j];
if (cpdur[j] > tpdur[j])
cpdur[j] = tpdur[j];
}
if (rate[j]>0 && tpdur[j]<cpdur[j]) {
cpdur[j] = cpdur[j] - rate[j];
if (cpdur[j] < tpdur[j])
cpdur[j] = tpdur[j];
}
}
// Initiate Channel 1 pulse
t1_preset = 65536 - (cpdur[1] *5);
set_timer1(t1_preset);
output_high(CH1_SP);
// Wait 20 ms while watching for incoming I2C messages
for (i=0; i<=1; i++) {
set_timer0(0); // Clear the servo pulse cycle timer
while (get_timer0() < 195) { // delay 10 ms
if (pdata_byte == 5) { // A full servo position command packet has been received
pdata_byte = 0;
if (cmd_channel >= 1 && cmd_channel <= 8) { // A valid channel has been commanded
// Calculate the commanded pulse duration and limit it
if (cmd_pdur > PD_MAX)
cmd_pdur = PD_MAX;
if (cmd_pdur < PD_MIN)
cmd_pdur = PD_MIN;
tpdur[cmd_channel] = cmd_pdur; // Set the pulse duration for the commanded channel
rate[cmd_channel] = cmd_rate; // Set the rate for the commanded channel
// If the commanded rate is 0 (no rate setting), set current pulse duration to commanded pulse duration.
if (cmd_rate == 0)
cpdur[cmd_channel] = cmd_pdur;
}
}
}
}
}
}
// Timer 1 overflow interrupt handler
// Timer 1 overflows at the end of the servo pulse duration
#INT_TIMER1
void t1_handler()
{
output_low(CH1_SP);
}
