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

	humanoid project RC servo control program

	pio_3.c
		control RC servo with button input
		<< THIS IS BAD EXAMPLE !! >>

	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-26	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 -------------------------------------------------------
#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 NO_EVENT					0x00
#define INC_BUTTON_EVENT			0x01
#define NEUT_BUTTON_EVENT			0x02
#define DEC_BUTTON_EVENT			0x04


// global variables	-----------------------------------------------------------
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);


// ============================================================================
//	main function
// ============================================================================
void main(void)
{
	int		event = NO_EVENT;
	
	while( 1 ){
		event = Button_Check();
		Servo_Update(event);
		LED_Update();
		delay_ms( SERVO_CONTROL_PERIOD_MS );
	}
}


// ----------------------------------------------------------------------------
//	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);

}