/************************************************************************
 * Title        srvtstme.c                                                 *
 * Programmer	Keith L. Doty	        				*
 * Date         May 08, 1999                                            *
 * Version	1			                		*
 *								        *
 * Description								*
 *   Input two integers to specify the speed and direction of the left  *
 *   and right wheels. The integers must lie between -100 and +100. Any *
 *   non-digit character will terminate the integer, e.g., a blank.     *
 *									*
 *   	      CAREFUL!!! The robot will start moving immediately!	*
 *									*
 * Use Hyperterm with the following settings:                           *
 *   									*
 *      COM1, 9600 Baud, 8 bits, No Parity, 1 Stop bit, No Flow         *
 *      of Control, VT100 Terminal, WrapLines.				*
 *									*
 *									*
 *   To input another pair of speeds, press reset to start the program  *
 *   over. 								*
 *									*
 *  Usage: This program allows you to move the robot at any fixed       *
 *         wheel speed ratios to generate either straight or circular	*
 *         motion.  							*
 ************************************************************************/
 
/**************************** Includes **********************************/
#include <tjpbase.h>
#include <stdio.h>
/************************ End of includes *******************************/


void main(void)
/****************************** Main ***********************************/
{
  int i,lspeed, rspeed, servo_index;
  unsigned int PW;
  
/* VT100 clear screen */ 
  char c1, clear[]= "\x1b\x5B\x32\x4A\x04"; 

/* VT100 position cursor at (x,y) = (3,12) command is "\x1b[3;12H"*/
  char place[]= "\x1b[1;1H";	/*Home*/	

  init_servome();
  init_motorme();
  init_serial();
  init_clocktjp();
  
while(1)
{

  printf("%s", clear);  
  printf("%s", place);
  
  printf("\tTitle\t\tsrvtstme.c\n"
  "\tProgrammer\tKeith L. Doty\n"
  "\tDate\t\tMay 06, 1999\n"
  "\tVersion\t\t1\n\n");


  for(i=0; i<NSERVO; i++)
  {
 	printf("Servo %d Lower Limit = %u\t\t\tServo %d Upper Limit = %u\n",
 	        i, servo_limits[i][0],servo_limits[i][1]);
  }

  printf("\n");

  for(i=0; i<NSERVO; i++)
  {

 	printf("Servo_width[%d] = %u\n",i, servo_width[i]);
 	
  }
  
  printf("\nEnter Servo Index: 0 to 4 (PA3-PA7)  ");
  servo_index = read_int();

  printf("Enter Servo Pulse Width: 1000<PW<4000 or PW=0:   ");
  PW = read_int();

  servo(servo_index, PW);
  
/*  
  printf("Enter percent of left motor speed, a number between -100 and 100:   ");
  lspeed = read_int();

  printf("Enter percent of right motor speed, a number between -100 and 100:  ");
  rspeed = read_int();

  motorme(RIGHT_MOTOR, rspeed); 
  motorme(LEFT_MOTOR, lspeed);
*/

  printf("Type any character to input new data.");
  c1 = get_char();
}

  
}
/**************************** End of Main ******************************/