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

 * Title        mstst.c                                	                

 * Programmer	Darren Kelley	        				

 * Date         April 9, 1999      	                                

 * Version	1			                		

 *								        

 * Description

 *   Allows the motors and servos to be tested for the ME11 board.

 *	 

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

 

/**************************** Includes **********************************/



#include <tkbase.h>

#include <stdio.h>



/************************ End of includes *******************************/

/**************************** Defines **********************************/





/************************ End of Defines *******************************/



/**************************** Prototypes **********************************/

void motor(int, int);



/**************************End of Prototypes ******************************/



/**************************** Globals **********************************/

 int run_test, mot_num, mot_sp, ser_num, degree;

/**************************End of globals ******************************/



/**************************  Main   ************************************/

void main(void)

{

/* Initialize the analog, motors, servo, clock, and serial */

  init_analog();

  init_motortk();

  init_clocktk();

  init_servos();

  init_serial();

	DDRD = 0xff;



 while(1)

 { 

	mot_sp = 0;

	degree = 0;

  printf("Choose one of the following options\n");

  printf("	1. Motors\n");

  printf("	2. Servos\n\n");

  run_test = read_int();

  if(run_test == 1)

  {

  	printf("Enter motor which you would like to test(0 or 1)\n");

  	mot_num = read_int();

  	while (mot_sp != 200)

  	{

  		printf("Enter motor speed(-100 to 100)-- 200 to exit\n");

  		mot_sp = read_int();

  		if (mot_sp != 200)

  		{

  		printf("You have choosen motor %d and speed %d\n", mot_num, mot_sp);

  		motor(mot_num, mot_sp);

  		}

  	}

  }

  else if (run_test == 2)

  {

  	printf("Enter the servo which you would like to test(0 or 1)\n");

  	ser_num = read_int();

  	while(degree != 100)

  	{

  		printf("Enter the servo degree(1000 to 5000)-- 100 to exit\n");

  		degree = read_int();

  		if (degree != 100)

  		{

  		servo(ser_num, degree);

  		}

  	}

  }

 } 	  

}  

 

void motor(int index, int percent)

/**************************** motor *****************************

 * Description

 * Sets the speed and direction for the motors.

 * 

 * 

 *								

 *Usage: to controls motor direction

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

{	

	if (index == 0)

	{	/* Set the Direction for motor0*/

		if ( percent < 0)

		{	SET_BIT(PORTD, 0x20);		/* SET PD5 to high */

			printf("you are in the motor function index 0\n");

			motortk(index, percent);	/* Call motor command*/

		}		

		else 

		{	CLEAR_BIT(PORTD, 0x20);	/* Set PD5 to low */

			motortk(index, percent);	/* Call motor command*/

		}

		

	}

	else if (index == 1)

	{	/* Set the direction of the motor1 */

		printf("you are in the motor function index 1\n");

		if ( percent < 0)

		{	SET_BIT(PORTD, 0x10);		/* Set PD4 to high*/

			motortk(index, percent);	/* Call motor command*/

		}		

		else 

		{	CLEAR_BIT(PORTD, 0x10);	/* Set PD4 to low*/

			motortk(index, percent);	/* Call motor command*/

		}

		

	}

}

/**************************** motor **** *****************************/