//===================================================================//
//                         SERVOTST.C                                //
//===================================================================//
//	By:		Dale Botkin <dale@botkin.org>					    //
//	Date:	3/15/2000                                             //
//-------------------------------------------------------------------//
// Simple C program for the PIC MPU, pretty much any variety.  Will  //
// exercise up to four R/C servos connected to pins RB0:3.  I tested //
// this with three, but it's pretty obvious you need to power the    //
// servos separately from the PIC to cut down on glitches.  Very     //
// easy to modify for up to 8 servos, though every servo you add     //
// sucks up another 10% of available CPU time.  Since all we're      //
// doing is exercising them, though, using 80% of CPU time running   //
// the pulses is no biggie.                                          //
//-------------------------------------------------------------------//
// Copyright (C)2000, Dale Botkin, all rights reserved.  You may use //
// this code freely for personal, non-commercial use.  In the highly //
// unlikely event that you want to use this code for commercial use, //
// email me to make arrangements.                                    //
//-------------------------------------------------------------------//


#include "16C711.H"					// Change for your CPU
#include "int16CXX.h"

#pragma config WDTE=off, FOSC=XT, BODEN=on
#pragma config |= 0x3FB0  			// Code protect off

char	servo_pin;					// Variable for port pins
char	servo[4];						// Servo position values
char	servo_num;					// Pointer to current servo position value
char	position;						// Working servo position value
char	window;						// 2ms - position value
char	boogies;						// Interrupt counter
char	loops;						// Loop counter
char	current;						// same as servo_num, but outside of interrupt
bit	direction;					// direction of travel flag

#pragma	origin 4

		// With .1ms resolution, we can control the servo in 10% steps:
		// 10 = 0 degrees (or 100% backward motion)		1.0ms
		// 11 = 18 degrees (or 80% backward motion)		1.1ms
		// 12 = 36 degrees (or 60% backward motion)		1.2ms
		// 13 = 54 degrees (or 40% backward motion)		1.3ms
		// 14 = 72 degrees (or 20% backward motion)		1.4ms
		// 15 = 90 degrees (or stopped)				1.5ms
		// 16 = 108 degrees (or 20% forward motion)		1.6ms
		// 17 = 126 degrees (or 40% forward motion)		1.7ms
		// 18 = 144 degrees (or 60% forward motion)		1.8ms
		// 19 = 162 degrees (or 80% forward motion)		1.9ms
		// 20 = 180 degrees (or 100% forward motion)		2.0ms
		// So our pulse routine needs to run 1ms + x tenths.
		// Next version will (maybe) use hundredths, from 100 to 200.

interrupt scan_servos() {
	PORTA = 2;									// indicate where we are
	T0IF = 0;										// Clear TMR0 interrupt flag
	int_save_registers								// Save status & W reg
	char fsr; fsr = FSR;							// Save FSR
	OPTION = 8;									// No TMR0 prescaler
	servo_pin = 1;									// Set up for first servo 
	for(servo_num = 0; servo_num <=3; servo_num++) {
		position = servo[servo_num];					// get pulse width value
		window = 30 - position;						// Figure out remainder of 2ms window
		PORTB = servo_pin;							// Turn on servo pin
	   	TMR0 = 0;									// Clear TMR0
		do {
			TMR0 = 11;							// Adjust for execution time
			while ( TMR0 < 100 );					// wait 100 uS
		} while( --position > 0 );					// for as many as we need
		PORTB = 0;								// Turn off all servo pins
			// Now we have to burn up the remainder of the 2ms total pulse
			// window...
		if(window > 0) {
			do {
					TMR0 = 11;					// Adjust for execution time
				while ( TMR0 < 100 );				// wait 100 uS
			} while(--window > 0);					// for as many as we need
		}
			// And set us up for the next servo in line...
		servo_pin = servo_pin*2;						// Set up for next servo in line
	}
		
		// Now we set up for an 8ms delay until the ext interrupt.
		// We just spent 8ms pulsing the servos, so in another 12 we
		// need to do it again.
	int_restore_registers							// Retrieve the registers
	FSR = fsr;									// Restore FSR
	OPTION = 5;									// 256 prescaler for TMR0
	TMR0 = 187;									// Set up for 12ms interrupt
	boogies++;									// Increment boogie counter
	PORTA = 0;
}
	

void main(){
	T0CS = 0;										// T0 on instruction cycle
	T0SE = 1;										// rising edge
	clearRAM();									// start with clear RAM
	PORTA = 0;						
	PORTB = 0;									// All outputs off
	TRISA = 0;
	TRISB = 0;									// Port B is all output
	OPTION = 5;									// Prescaler set to 256
	direction = 0;										
	
		// Set all servo position registers to the 90 degree mid point.
	for(current = 0; current < 4; current++) {
		servo[current] = 15;
	} 
	TMR0 = 131;									// Set up timer for 8ms
	T0IE = 1;										// Enable TMR0 interrupt
	GIE = 1;										// Duh.

		// The boogie counter gets updated every 20ms, so 50 boogies
		// equals one second.  Tip of the hat to bogomips.

		// First wait 2 seconds to let us see how the servos do at deadband
	while(boogies < 100);							// Loop for 2 seconds

		// Now do 60 loops of 10% steps once a second, full motion
		// range from 10 to 20 and back.  Should take 1 minute.
	direction = 1;
	loops = 0;
	PORTA = 1;
	do {
		boogies = 0;								// clear boogie counter
		while( boogies < 10 );						// Wait 1/5 second	
		for(current=0; current < 4; current++) {
			if(servo[current] == 25) direction = 0;
			if(servo[current] == 5) direction = 1;
			if(direction == 0) servo[current]--;
			if(direction == 1) servo[current]++;
		}
	} while(++loops < 60);

		// Now recenter the servos for 2 seconds 
	for(current=0; current < 4; current++) {
		servo[current] = 15;
	}		
	boogies = 0;
	while(boogies < 100);

		// Now we finish up with 10 full-travel swings, 10 to 20,
		// with a 2-second delay in between. 

	loops = 0;
	PORTA = 0xD;
	do {
		boogies = 0;
		while(boogies < 100);						// 2 second delay
		for(current=0; current < 4; current++) {
			if(servo[current] == 25) direction = 0;
			else if(servo[current] == 5) direction = 1;
			else if(servo[current] > 5 && servo[current] < 25) direction = 0;
			if(direction == 0) servo[current] = 5;
			if(direction == 1) servo[current] = 25;
		}
	} while(++loops < 10);

		// And loop forever.
	main();
}

  .