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

	humanoid project RC servo control program

	servo5.c
		control multi RC servo with VB program

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

/*
	peripheral setting

		RB0-7	---	LED 0-7
		RC0-3	--- servo 0-3
		RC6(TX)	--- T1in of MAX232CPE (RS-232C driver IC)
		RC7(RX)	--- R1out of MAX232CPE
*/

//	history
//	2004-01-25	ver.0.5
//	2003-03-08	ver.0

#include <16f876a.h>

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

#use delay(CLOCK = 20000000)
#use rs232(BAUD = 19200, XMIT = PIN_C6, RCV = PIN_C7)

//	peripheral definition -----------------------------------------------------
#define SERVO_0						PIN_C0
#define SERVO_1						PIN_C1
#define SERVO_2						PIN_C2
#define SERVO_3						PIN_C3

#define BUF_SIZE						15

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

#define TASK_STATE_WAIT				0x00
#define TASK_STATE_READY				0x01

#define TASK_WUP_PERIOD_INF			-1

// 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		// 1ms


#define SERVO_TASK_PERIOD_MS		20		// servo task 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 COMM_RECEIVE_EVENT			0x08

#define	LED_TASK_PERIOD_MS			200		// LED task period 200ms


// type definision	-----------------------------------------------------------

// task control block
typedef struct{
	char		state;				// task state
	signed long	wupPeriod;			// wake up period (16bit)
	signed long	wupCnt;				// wake up count
	//	Because CSS-PCM compiler does not permit pointer to function,
	//	TCB(task control block) and tasks are connected with task ID.
}t_tcb;

// task ID
enum {
	SERVO_0_TASK,
	SERVO_1_TASK,
	SERVO_2_TASK,
	SERVO_3_TASK,
	COMM_TASK,
	LED_TASK,
	TASK_NUM
}taskID;

// communication packet structure
typedef struct{

	char	head;		//	head of comm packet is "@"
	char	servoID;
	char	sign;		//	positive or negative sign of data
	char	data_10;
	char	data_1;
	char	tail;		//	tail of comm packet is [LF] (\n)
	int		ok;
	
}t_Packet;

// servo structure
typedef struct{
	signed int	angle;
	int			isActive;
}t_Servo;

// global variables	-----------------------------------------------------------
t_tcb			tcb[TASK_NUM];
t_Packet			Packet = { 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, FALSE };	// dummy initialize
t_Servo			Servo[4];

unsigned int		sysClock = 0;
unsigned int		event = NO_EVENT;				// event flag

char			commBuffer[BUF_SIZE];
int				commPtr = 0;

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

// event check function
unsigned int CheckEvent(void);

// tasks
void ServoTask(int i);
void CommTask(void);
void LedTask(void);

// servo related function
void Servo_Init(int i);
void Servo_SetAngle(int i, int a);
signed int Servo_GetAngle(int i);
void Servo_Output(int i);

// RS-232C related function
void Comm_AnalyzePacket(void);



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

	sysClock++;
	if( 0 == (0xFF - sysClock) )
		sysClock = 0;
}

// ============================================================================
//	servo low output timer (timer1 interrupt handler)
// ============================================================================
#int_timer1
void ServoLowTimer(void)
{
	int i;

	for(i=0; i<4; i++){
		if( TRUE == Servo[i].isActive ){
			Servo[i].isActive = FALSE;

			switch(i){
			case 0:	output_low( SERVO_0 );	break;
			case 1:	output_low( SERVO_1 );	break;
			case 2:	output_low( SERVO_2 );	break;
			case 3:	output_low( SERVO_3 );	break;
			}
		}
	}
	
	setup_timer_1( T1_DISABLED );
	disable_interrupts( INT_TIMER1 );
}


// ============================================================================
//	serial communication receive interrupt handler
// ============================================================================
#int_rda
void Rs232c_Receive(void)
{	
	while( kbhit() ){
		commBuffer[ commPtr ] = getc();
		commPtr++;
	}	
	tcb[COMM_TASK].state = TASK_STATE_READY;	
}



// ============================================================================
//	main function ( work as scheduler )
// ============================================================================
void main(void)
{

	int i;
	unsigned int	lastClock;


	// create tasks ---------------------------------------

	// servo task
	tcb[SERVO_0_TASK].state		= TASK_STATE_WAIT;	
	tcb[SERVO_0_TASK].wupPeriod 	= SERVO_TASK_PERIOD_MS;
	tcb[SERVO_0_TASK].wupCnt		= 0;

	tcb[SERVO_1_TASK].state		= TASK_STATE_WAIT;
	tcb[SERVO_1_TASK].wupPeriod 	= SERVO_TASK_PERIOD_MS;
	tcb[SERVO_1_TASK].wupCnt		= SERVO_TASK_PERIOD_MS / 4;

	tcb[SERVO_2_TASK].state		= TASK_STATE_WAIT;
	tcb[SERVO_2_TASK].wupPeriod 	= SERVO_TASK_PERIOD_MS;
	tcb[SERVO_2_TASK].wupCnt		= SERVO_TASK_PERIOD_MS / 4 * 2;

	tcb[SERVO_3_TASK].state		= TASK_STATE_WAIT;
	tcb[SERVO_3_TASK].wupPeriod 	= SERVO_TASK_PERIOD_MS;
	tcb[SERVO_3_TASK].wupCnt		= SERVO_TASK_PERIOD_MS / 4 * 3;	

	// (SERVO_TASK_PERIOD_MS / 4) should be enough longer than
	//		(SERVO_ON_WIDTH_US_PER_DEG * MAX_SERVO_ANGLE + SERVO_NEUTRAL_ON_WIDTH_US)

	for(i=0; i<4; i++)
		Servo_Init(i);
		
	tcb[COMM_TASK].state		= TASK_STATE_WAIT;
	tcb[COMM_TASK].wupPeriod	= TASK_WUP_PERIOD_INF;
	tcb[COMM_TASK].wupCnt		= TASK_WUP_PERIOD_INF;

	// LED task
	tcb[LED_TASK].wupPeriod	= LED_TASK_PERIOD_MS;
	tcb[LED_TASK].wupCnt		= 0;


	// 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( INT_RDA );
	enable_interrupts( GLOBAL );	


	// body of task scheduler -----------------------------
	lastClock = sysClock;
	while(1){


		if( 0 != (sysClock - lastClock) ){
			for(i=0; i<TASK_NUM; i++){
				if( 0 < tcb[i].wupCnt )
					tcb[i].wupCnt--;
				
				if( 0 == tcb[i].wupCnt )
					tcb[i].state = TASK_STATE_READY;
			}
			lastClock = sysClock;
		}

		for(i=0; i<TASK_NUM; i++){
			if( TASK_STATE_READY == tcb[i].state ){

				switch( i ){
				case SERVO_0_TASK:	ServoTask(0);	break;
				case SERVO_1_TASK:	ServoTask(1);	break;
				case SERVO_2_TASK:	ServoTask(2);	break;
				case SERVO_3_TASK:	ServoTask(3);	break;
				case COMM_TASK:		CommTask();	break;
				case LED_TASK:		LedTask();	break;
				default:		break;//	SystemDown();
				}

				tcb[i].state  = TASK_STATE_WAIT;
				tcb[i].wupCnt = tcb[i].wupPeriod;
			}
		}
	}
	// ----------------------------------------------------

}

// ----------------------------------------------------------------------------
//	event check function
// ----------------------------------------------------------------------------
unsigned int CheckEvent(void)
{
	if( event & COMM_RECEIVE_EVENT ){
		event &= ~COMM_RECEIVE_EVENT;	// clear event flag		
		return COMM_RECEIVE_EVENT;
	}
	return NO_EVENT;
}

// ----------------------------------------------------------------------------
//	servo control task
// ----------------------------------------------------------------------------
void ServoTask(int i)
{		
	Servo_Output(i);
}

void Servo_Init(int i)
{
	Servo[i].angle = 0;
	Servo[i].isActive = FALSE;
}


void Servo_SetAngle(int i, int a)
{
	Servo[i].angle = a;	
	
	if( MAX_SERVO_ANGLE < Servo[i].angle )
		Servo[i].angle = MAX_SERVO_ANGLE;
	else if( MIN_SERVO_ANGLE > Servo[i].angle )
		Servo[i].angle = MIN_SERVO_ANGLE;

}

signed int Servo_GetAngle(int i)
{
	return Servo[i].angle;
}

void Servo_Output(int i)
{
	unsigned long onWidth, t1_cnt;

	onWidth = SERVO_ON_WIDTH_US_PER_DEG * (signed long)Servo[i].angle
				+ SERVO_NEUTRAL_ON_WIDTH_US;

	t1_cnt = 0xffff - 5 * onWidth;
	// here, 5 = 1us / (50ns * 4)

	Servo[i].isActive = TRUE;

	switch( i ){
	case 0:	output_high( SERVO_0 );	break;
	case 1:	output_high( SERVO_1 );	break;
	case 2:	output_high( SERVO_2 );	break;
	case 3:	output_high( SERVO_3 );	break;
	default:	break;
	}

	setup_timer_1( T1_INTERNAL | T1_DIV_BY_1 );
	set_timer1( t1_cnt );
	enable_interrupts( INT_TIMER1 );
}


// ----------------------------------------------------------------------------
//	LED control task
// ----------------------------------------------------------------------------

void LedTask(void)
{
	int i, angle;
	unsigned int8	temp;
	unsigned int8	ledPtn;

	ledPtn = 0;

	// show servo i is MAX or MIN angle
	for(i=0; i<4; i++){
		angle  = Servo_GetAngle(i);
		if( MAX_SERVO_ANGLE == angle )
			temp = 2;
		else if( MIN_SERVO_ANGLE == angle )
			temp = 1;
		else
			temp = 0;

		ledPtn += temp << (i*2);
	}

	//	example
	//	00 00 00 00		all servo angle	OK
	//	00 00 00 01		servo 0 angle MIN
	//	10 00 00 00		servo 3 angle MAX
	
	output_b(ledPtn);

}


// ----------------------------------------------------------------------------
//	RS-232C communication task
// ----------------------------------------------------------------------------
void CommTask(void)
{
	disable_interrupts( INT_RDA );	
	
	if( 0x0a == commBuffer[ commPtr-1 ]  ){				// check tail of packet
		if( '@' == commBuffer[0] ){			// check head of packet
			Packet.servoID	= commBuffer[1];				
			Packet.sign		= commBuffer[2];
			Packet.data_10 	= commBuffer[3];
			Packet.data_1  	= commBuffer[4];
			Packet.ok		= TRUE;
			
			Comm_AnalyzePacket();
		}
						
		commPtr = 0;
		event |= COMM_RECEIVE_EVENT;
		break;
	}

	enable_interrupts( INT_RDA );	
}

void Comm_AnalyzePacket(void)
{
	int i, a;

	if( FALSE == Packet.ok )
		return;

	i = (int)(Packet.servoID - 0x30);

	if( 0x30 != Packet.data_10 )
		a = 10 * ( Packet.data_10 - 0x30 );
	else
		a = 0;

	if( 0x30 != Packet.data_1 )
		a += Packet.data_1 - 0x30;

	if( '-' == Packet.sign )
		a = -a;
			
	if( TRUE == Packet.ok ) {
		Servo_SetAngle(i, a);
		Packet.ok = FALSE;
	}
}