/**********************************************************************************
 *  Program Name: 8servo control
 *  Author      : atsushi hasegawa
 *  Date        : 2006-12-21
 *  explanation : test program for 4leg robot
 *  caution	: set Stack-Location #2
 *				 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 30	//base for motion interval adjustment
#define PWM_BASE 9	//base for PWM adjustment

/*
4- 3-|F|-1 -2
     | |
8- 7-|B|-5 -6
*/
#define SERVO_1	CN1	//servo-CN matching
#define SERVO_2	CN2
#define SERVO_3	CN5
#define SERVO_4	CN6
#define SERVO_5	CN7
#define SERVO_6	CN8
#define SERVO_7	CN9
#define SERVO_8	CN10

byte walk_repeat;

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

void servo(byte deg1, byte deg2, byte deg3, byte deg4, byte deg5, byte deg6, byte deg7, byte deg8, byte motion_time);

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

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

	while(1){
		servo(15,15,15,15,15,15,15,15,3);
		DELAY_SEC(50);		
		for(walk_repeat=0;walk_repeat<=5;walk_repeat++){
			servo(15,13,15,15,15,15,15,13,1);
			servo(14,13,13,15,17,15,16,13,1);
			servo(15,15,15,17,15,17,15,15,1);
			servo(17,15,16,17,14,17,13,15,1);			
			COPCTL = 0;
		}

		COPCTL = 0;
	}
}

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

void servo(byte deg1, byte deg2, byte deg3, byte deg4, byte deg5, byte deg6, byte deg7, byte deg8, byte motion_time){
	word i;
	word m;
	byte n;
	word p;

	motion_time*=T_BASE;
	for(m=0;m<=motion_time;m++){
		for(n=1;n<=8;n++){
			if(n==1){p=PWM_BASE*deg1,SERVO_1=1;}
			if(n==2){p=PWM_BASE*deg2,SERVO_1=0,SERVO_2=1;}
			if(n==3){p=PWM_BASE*deg3,SERVO_2=0,SERVO_3=1;}
			if(n==4){p=PWM_BASE*deg4,SERVO_3=0,SERVO_4=1;}
			if(n==5){p=PWM_BASE*deg5,SERVO_4=0,SERVO_5=1;}
			if(n==6){p=PWM_BASE*deg6,SERVO_5=0,SERVO_6=1;}
			if(n==7){p=PWM_BASE*deg7,SERVO_6=0,SERVO_7=1;}
			if(n==8){p=PWM_BASE*deg8,SERVO_7=0,SERVO_8=1;}
			for(i=0;i<=p;i++){
				COPCTL = 0;	
			}
			COPCTL = 0;
		}
		p=PWM_BASE*40,SERVO_8=0;
		for(i=0;i<=p;i++){
			COPCTL = 0;	
		}
		COPCTL = 0;
	}
}