Động cơ RC Servo

[opto]

đây là code cho 1 servo:

#include "16F84.H"
#define CP_off |= 0x3FFF // copy protect off for 16F84
#pragma config CP_off, PWRTE=on, WDTE=off, FOSC=XT

/* assign names to ports and pins */
#pragma bit SERVO @ PORTB.1
#pragma bit RESET_S @ PORTA.0
#pragma bit TURN_C @ PORTA.1
#pragma bit TURN_A @ PORTA.2

/* function prototypes */
void delay_min (void);
void delay_var (char n);
void pause (void);

void main( void)
{
PORTB = 0b.0000.0000; /* initial value */
TRISB = 0b.0000.0000; /* Port B all o/p */
PORTA = 0b.0.0111; /* initial value */
TRISA = 0b.1110.0111; /* xxx0 0001 */

char position=107; // total time 1.5ms center position

while(1) // endless loop
{
SERVO=1;
delay_min();
if (position>0)
delay_var(position);
SERVO=0;
pause(); // 20 ms delay before next pulse
if ((TURN_C==0)&&(position<255))
position++;
if ((TURN_A==0)&&(position>0))
position--;
if (RESET_S==0)
position=107; // center the servo
}

} // end of main


void delay_min (void) // 750 uS including overhead
{
char n=248;
do ; while(--n>0);
nop(); // padding to produce precise time
}
// delay = 2 + 2 + (n x 3) -1 +1 +2 = 750us @ 4MHz
// 2 for call, 2 for preset n, (n x 3)-1 for loop, 1 padding, 2 for return

void delay_var (char n) // adc x 7 uS = 1790us max.
{
do {nop(); nop(); nop(); nop();} while(--n>0);
}
// delay= 2 + 2 + (t x 7) -1 +2 us @ 4MHz

void pause (void) // delay between pulses approx 20 ms
{
char n;
for (n=0;n<26;n++)
{
delay_min(); // 750 us
}
}