/**********************************************************************************
 *  Program Name:servo control test
 *  Author      :atsushi hasegawa
 *  Date        : 2006-12-21
 *  explanation :test program for servo control
 *               
 *				 JAPANROBOTECH C-Language Development Environment
 **********************************************************************************/

#include "JRT102.h"      // Hardware definition
#include "RoboDesigner.h"// RoboDesigner Hardware definition
#include "TiCollaLib.h"  // Library definition
#include "vectors.h"     // Interruption vector definition
#include "initialize.h"  // Ininitalize
#include "interrupt.h"   // Interruption


/*--------------------------------------------------------------------------------*/
/* Variable declaration                                                           */        
/*--------------------------------------------------------------------------------*/

//pulse to pulse:10-20msec/pulse width:0.7-2.3msec center=1.5msec
#define T_BASE 100	//base for motion interval adjustment
#define PWM_BASE 9	//base for PWM adjustment

#define SERVO_1	CN7	//servo-CN matching

/*--------------------------------------------------------------------------------*/
/* Prototype declaration                                                          */        
/*--------------------------------------------------------------------------------*/

void servo(byte deg, byte motion_time);

/*--------------------------------------------------------------------------------*/
/*  main                                                                          */
/*--------------------------------------------------------------------------------*/

void main(void) {
	
	init();		//Initialization

	while(1){
		servo(14,1);
		servo(16,2);

		COPCTL = 0;	
	}
}

/*--------------------------------------------------------------------------------*/
/*  function                                                                      */
/*--------------------------------------------------------------------------------*/

void servo(byte deg, byte motion_time){
	word i;
	word m;

	motion_time*=T_BASE;
	for(m=0;m<=motion_time;m++){
		SERVO_1=1;
		for(i=0;i<=PWM_BASE*deg;i++){
			COPCTL = 0;	
		}
		SERVO_1=0;
		for(i=0;i<=PWM_BASE*120;i++){
			COPCTL = 0;	
		}
		COPCTL = 0;
	}
}