/******************************************************************************

	humanoid project RC servo control program

	interrupt_1a.c
		control RC servo with button input
		<< little bit fixed version >>

	Author		:	Takashi Tomiyama ( yij01260@nifty.com )
	Target		:	PIC16F876A
	Clock		:	20MHz
	Compiler	:	CSS-PCM
	
******************************************************************************/

/*
	peripheral setting

		RA3-5	--- button 0,1,2
		RB0-7	---	LED 0-7
		RC0		--- servo 0
*/

//	history
// 	2003-03-28	ver.0

#include <16f876a.h>

#fuses	HS, NOWDT, NOPROTECT, PUT, BROWNOUT, NOLVP

#use	delay(CLOCK = 20000000)

//	peripheral definition -----------------------------------------------------
#define INC_BUTTON					PIN_A3
#define NEUT_BUTTON					PIN_A4
#define DEC_BUTTON					PIN_A5

#define SERVO_0						PIN_C0

//	constant definition -------------------------------------------------------

// how derive count setting 1ms interval of timer0
//		0xff - (interval time) / (cpu clock * 4) / (priscaler)
//	=	0xff - 1ms / (50ns * 4) / 64
//	=	0xff - 1000000ns / 200ns / 64
//	=	0xff - 5000 / 64
//	=	0xff - 0x4e					(78=0x4e)
//	=	0xb1

#define	TIMER0_CNT					0xb1

#define SERVO_CONTROL_PERIOD_MS		15		// servo control period 15ms
#define SERVO_ON_WIDTH_US_PER_DEG	9		// coefficient of on time width [us/deg]
#define SERVO_NEUTRAL_ON_WIDTH_US	1500	// servo neutral time width 1500us
#define MAX_SERVO_ANGLE				65		// max servo angle 65 degree
#define MIN_SERVO_ANGLE				-65		// min servo angle -65 degree

#define BUTTON_CHECK_PERIOD_MS		100		// button check period 100ms
#define NO_EVENT					0x00
#define INC_BUTTON_EVENT			0x01
#define NEUT_BUTTON_EVENT			0x02
#define DEC_BUTTON_EVENT			0x04

#define	LED_CONTROL_PERIOD_MS		100		// LED control period 100ms

// global variables	-----------------------------------------------------------
unsigned int	servoWaitCountMs;
unsigned int	buttonWaitCountMs;
unsigned int	ledWaitCountMs;


signed int servoAngle = 0;				// servo output angle [deg];


// prototype declaration ------------------------------------------------------

// servo related function
void Servo_Update(int event);
void Servo_IncAngle(int a);
void Servo_DecAngle(int a);
void Servo_SetAngle(int a);
signed int Servo_GetAngle(void);
void Servo_Output(void);


// button related function
int Button_Check(void);


// LED related function
void LED_Update(void);


// ============================================================================
//	interval timer (timer0 interrupt hander)
// ============================================================================
#int_timer0
void IntervalTimer(void)
{	
	set_timer0( TIMER0_CNT );	// 1ms interval

	if( 0 < servoWaitCountMs )
		servoWaitCountMs--;

	if( 0 < buttonWaitCountMs )
		buttonWaitCountMs--;

	if( 0 < ledWaitCountMs )
		ledWaitCountMs--;
}

// ============================================================================
//	main function
// ============================================================================
void main(void)
{
	int		event = NO_EVENT;

	servoWaitCountMs  = SERVO_CONTROL_PERIOD_MS;
	buttonWaitCountMs = BUTTON_CHECK_PERIOD_MS;
	ledWaitCountMs    = LED_CONTROL_PERIOD_MS;

	// setup interrupt timer0 -----------------------------
	setup_timer_0( RTCC_INTERNAL | RTCC_DIV_64 );
	set_timer0( TIMER0_CNT );	// 1ms interval

	// enable interrupt
	enable_interrupts( INT_TIMER0 );
	enable_interrupts( GLOBAL );	

	// endless loop ---------------------------------------
	while( 1 ){

		if( 0 == servoWaitCountMs ){
			servoWaitCountMs = SERVO_CONTROL_PERIOD_MS;
			Servo_Update(event);
		}

		if( 0 == buttonWaitCountMs ){
			buttonWaitCountMs = BUTTON_CHECK_PERIOD_MS;
			event = Button_Check();
		}

		if( 0 == ledWaitCountMs ){
			ledWaitCountMs    = LED_CONTROL_PERIOD_MS;
			LED_Update();
		}
		
	}
}


// ----------------------------------------------------------------------------
//	servo related function
// ----------------------------------------------------------------------------
void Servo_Update(int event)
{

	if( NEUT_BUTTON_EVENT & event )
		Servo_SetAngle( 0 );
		
	else if( INC_BUTTON_EVENT & event )
		Servo_IncAngle( 1 );

	else if( DEC_BUTTON_EVENT & event )
		Servo_DecAngle( 1 );


	Servo_Output();
}

void Servo_IncAngle(int a)
{
	servoAngle += a;
	if( MAX_SERVO_ANGLE < servoAngle )
		servoAngle = MAX_SERVO_ANGLE;
}

void Servo_DecAngle(int a)
{
	servoAngle -= a;
	if( MIN_SERVO_ANGLE > servoAngle )
		servoAngle = MIN_SERVO_ANGLE;
}

void Servo_SetAngle(int a)
{
	servoAngle = a;

	if( MAX_SERVO_ANGLE < servoAngle )
		servoAngle = MAX_SERVO_ANGLE;
	else if( MIN_SERVO_ANGLE > servoAngle )
		servoAngle = MIN_SERVO_ANGLE;
}

signed int Servo_GetAngle(void)
{
	return servoAngle;
}

void Servo_Output(void)
{
	int i;
	long onWidth, onWidthDiv10;

	onWidth = SERVO_ON_WIDTH_US_PER_DEG * (signed long)servoAngle
				+ SERVO_NEUTRAL_ON_WIDTH_US;

	onWidthDiv10 = onWidth / 10;

	output_high( SERVO_0 );
	for(i=0; i<onWidthDiv10; i++)
		delay_us(10);
		
	output_low( SERVO_0 );				
}


// ----------------------------------------------------------------------------
//	button related function
// ----------------------------------------------------------------------------

int Button_Check(void)
{
	int	i;
	int cnt;
	int pushedPtn = NO_EVENT;

	// Incriment button
	cnt = 3;
	for(i=0; i<3; i++)
		cnt -= input( INC_BUTTON );

	if( 3 == cnt )
		pushedPtn += INC_BUTTON_EVENT;

	// Neutral button
	cnt = 3;
	for(i=0; i<3; i++)
		cnt -= input( NEUT_BUTTON );

	if( 3 == cnt )
		pushedPtn += NEUT_BUTTON_EVENT;

	// Decriment button
	cnt = 3;
	for(i=0; i<3; i++)
		cnt -= input( DEC_BUTTON );

	if( 3 == cnt )
		pushedPtn += DEC_BUTTON_EVENT;		

	return pushedPtn;
}

// ----------------------------------------------------------------------------
//	LED related function
// ----------------------------------------------------------------------------

void LED_Update(void)
{
	signed int angle, quot;
	unsigned int8 ledPtn;
	
	angle  = Servo_GetAngle();
	ledPtn = 0x18;

	//  65[deg]  11110000
	//  60[deg]  11000000
	//  40[deg]  01100000
	//  20[deg]  00110000
	//   0[deg]  00011000
	// -20[deg]  00001100
	// -40[deg]  00000110
	// -60[deg]  00000011
	// -65[deg]  00001111

	if( MAX_SERVO_ANGLE == angle )
		ledPtn = 0xf0;
	else if( 0 < angle ){
		quot = angle / 20;	
		ledPtn = ledPtn << quot;
	}
	else if( 0 > angle && MIN_SERVO_ANGLE < angle){
		quot = -angle / 20;
		ledPtn = ledPtn >> quot;
	}
	else if( MIN_SERVO_ANGLE == angle)
		ledPtn = 0x0f;

	output_b(ledPtn);

}