#include <90S8535.h>
#include <stdio.h>  
#include <stdlib.h> 
#include <delay.h>  
#include <string.h>     

/*
   controller connections to port C
   0  = VCC/GND
   1 = Gripper
   2 = Elbow
   3 = Base
   4 = Dummy
   5 = Shoulder
   6 = Wrist
   7 = GND/GCC     

   motor switches connections to port D & port B & PORT A
   these are active low
   PORTB 
   0, 1 Wrist up & down switches
   2, 3 Shoulder up & down switches
   4, PORTA 5 Base up & down switches
   
   PORT A 
   6, 7 Elbow up & down switches
   
   PORT D
   6,7 Gripper up & down switches 
   
   Control switches
   PORTA.3 = training period switch
   PORTA.4 = replay switch
      
   Sensors connections
   PIND.3, 2 =  wrist photosensor
   PIND.5, 4 =  Base photosensor
   
   ADC sensor connections
   PINA.0 = gripper potentiometer       
   PINA.1 = elbow potentiometer
   PINA.2 = shoulder potentiometer

 */

// alias names (Port assignments)     
#define CONTROLLER_DIR 		DDRC
#define CONTROLLER_IN 		PINC   			// input 
#define CONTROLLER_OUT 		PORTC  			// output   

// output switches                     
#define MOTOR_DIR	       	DDRB
#define GRIPPER_UP_DIR 	    DDRD.6
#define GRIPPER_DOWN_DIR    DDRD.7

#define MOTOR		      	 	PORTB
#define WRIST_UP_SWITCH 		PORTB.0
#define WRIST_DOWN_SWITCH 		PORTB.1
#define SHOULDER_UP_SWITCH 		PORTB.2
#define SHOULDER_DOWN_SWITCH 	PORTB.3
#define BASE_UP_SWITCH 	   		PORTB.4
#define BASE_DOWN_SWITCH 		PORTA.5 	//PORTB.5
#define ELBOW_UP_SWITCH 		PORTA.6		//PORTB.6
#define ELBOW_DOWN_SWITCH 		PORTA.7		//PORTB.7
#define GRIPPER_UP_SWITCH 		PORTD.6
#define GRIPPER_DOWN_SWITCH 	PORTD.7

// input sensors & switches
#define SENSOR_DIR	   		DDRD
#define WRIST_SENSOR_L		PIND.2 // change back to PIND.2
#define WRIST_SENSOR_R		PIND.3 // change back to PIND.3
#define BASE_SENSOR_L		PIND.4
#define BASE_SENSOR_R		PIND.5

#define TRAINING_SWITCH		PINA.3 
#define REPLAY_SWITCH		PINA.4 // test with PIND.2 for easy access to button
   
#define ADC_SENSOR          	PORTA
#define SHOULDER_SENSOR 	PINA.2
#define ELBOW_SENSOR 		PINA.1
#define GRIPPER_SENSOR 		PINA.0

/* motor number */
#define GRIPPER  1
#define BASE     2
#define ELBOW    3
#define SHOULDER 4
#define WRIST    5
                        
/* boolean logic */
#define TRUE  1
#define FALSE 0

/* the motor directions */
#define IDLE	2  	
#define UP	1	          
#define DOWN    0      
                        
/* motor switches (active low) */
#define ON  0
#define OFF 1      

/* The operation modes */
#define NORMAL   0
#define TRAINING 1
#define REPLAY   2
                     
/* base direction */
#define LEFT 	   0
#define RIGHT	   1 
#define STATIONARY 2

/* interval timing stuff */
#define SCAN_INTERVAL 50 	// scan every 50 ms
     

/* function prototypes */
void initialize(void);          // init everything  
void scan(void);                // scan the input
void output(void);              // output to the motor                         
int performConversion(int);	// perform ADC conversion
void record(void);              // record the motion
void recordFinal(void);		// record the final position from potentiometer
void play(void);		// replay the data       
void playForward(void);
 

/* variables */          
// timer stuff
unsigned char timer0_reload;   // timer 0 reload time
unsigned char scan_time;

// manual controller input
unsigned char motor_grip, motor_elbow, motor_base, motor_shoulder, motor_wrist;       
unsigned char motor_up, motor_down; 

// photosensors input
unsigned char first_time;  
char shoulder_done = 0, elbow_done = 0, gripper_done = 0, wrist_done = 0, base_done = 0;  

unsigned char wrist_old_inputL, wrist_old_inputR;
unsigned char wrist_curr_inputL, wrist_curr_inputR;
signed char wrist_pulse, temp_wrist_pulse;		// # of pulses can go either way
unsigned char wrist_dir;

unsigned char base_old_inputL, base_old_inputR;
unsigned char base_curr_inputL, base_curr_inputR;
signed char base_pulse, temp_base_pulse;		// # of pulses can go either way
unsigned char base_dir;


// potentiometer stuff
unsigned char converted;                 
int voltage, upper_volt;
int init_gripper_volt, end_gripper_volt;
int init_elbow_volt, end_elbow_volt;
int init_shoulder_volt, end_shoulder_volt; 

// control switches         
unsigned char mode, old_mode;
unsigned char training;
unsigned char replay;


/* timer 0 overflow ISR -- setup the intervals for manual controller check */					
interrupt [TIM0_OVF] void timer0_overflow(void)
{                              
	if(scan_time > 0) scan_time--;
}

/* ADC conversion ready ISR */
interrupt [ADC_INT] void adc_interrupt(void)
{
 	converted = 1;
}

          
 /* main controls everything */        
void main (void)    
{          
	initialize();	// initialize  
	while(1)     
	{          
	    if(scan_time == 0)
	    { 
	      // normal or training mode	    
	      if(mode != REPLAY)	
	      {
	      	scan(); 
	      	if(motor_up == 1 || motor_down == 1) 
	      	{        
	      	   
	       	  printf("\r\n");
	       	  printf("motor_grip = %d\r\n", motor_grip);
	       	  printf("motor_elbow = %d\r\n", motor_elbow);
	       	  printf("motor_base = %d\r\n", motor_base);
	       	  printf("motor_shoulder = %d\r\n", motor_shoulder);
	       	  printf("motor_wrist = %d\r\n", motor_wrist);  
	       	  printf("motor_up = %d\r\n", motor_up);
	       	  printf("motor_down = %d\r\n", motor_down);
	       	                           

	       	  output();  		                        		 
		}      
		                                       
		 if(mode == TRAINING) 
		 {
		    old_mode = TRAINING;
		    record();   	    		   	   	      	          
		 } // else if just back to normal mode for training                      
		 else if(old_mode == TRAINING)
		 {                 
		    old_mode = NORMAL;
		    recordFinal();
		 }
		    
	       }   
	       else  // replay mode
	       {
	         play();       
	         shoulder_done = 0; elbow_done = 0; gripper_done = 0; wrist_done = 0; base_done = 0;     
	         temp_base_pulse = base_pulse;
		     temp_wrist_pulse = wrist_pulse;
	         delay_ms(1000); 	// delay 1 s before doing the movement
	         playForward(); 	       	 
	       }               
	     }
	     else // turn off the motor
	     {
	        MOTOR = 0xff;      
	    	GRIPPER_UP_SWITCH   = 1;
	     	GRIPPER_DOWN_SWITCH = 1;   
	  	 	ELBOW_UP_SWITCH = 1;
	   		ELBOW_DOWN_SWITCH = 1;
			BASE_UP_SWITCH = 1;
	        BASE_DOWN_SWITCH = 1;
	     }    
	}    		    
	
}

/* scan manual controller for input */
void scan(void)
{     	    
  // setup interval for next time
  scan_time = SCAN_INTERVAL;
   
  // scan for training switch & replay switch

  training = (TRAINING_SWITCH == 0)? 1 : 0;    
  replay = (REPLAY_SWITCH  == 0) ? 1 : 0;
   
  /* determine the mode that we are in */
  if(training)
  { 
    mode = TRAINING;            
    if(old_mode == NORMAL)
	    printf("TRAINING MODE\r\n");
	    
  }
  else if(replay)
  {
     mode = REPLAY;
     first_time = TRUE;    
     printf("REPLAY MODE \r\n");    
     temp_base_pulse = base_pulse;
     temp_wrist_pulse = wrist_pulse;
     shoulder_done = 0; elbow_done = 0; gripper_done = 0; wrist_done = 0; base_done = 0;
  }
  else
  {
    mode = NORMAL;
    first_time = TRUE;          
  } 
  	
  
  // scan for up input      
  CONTROLLER_DIR  = 0x81;         //  output for bit 0 and 7, rest as input (10000001)
  CONTROLLER_OUT = 0xfe;         // pin 0 = Gnd, pin 7 = Vdd, rest turns on pull-ups  (1111 1110)         
  delay_us(5);  
  motor_grip     = CONTROLLER_IN.1 == 0 ? UP : IDLE;   
  motor_elbow    = CONTROLLER_IN.2 == 0 ? DOWN : IDLE;   
  motor_base     = CONTROLLER_IN.3 == 0 ? DOWN : IDLE;   
  motor_shoulder = CONTROLLER_IN.5 == 0 ? UP : IDLE;   
  motor_wrist    = CONTROLLER_IN.6 == 0 ? UP : IDLE;
            	  
  //scan for down input   
  CONTROLLER_OUT = 0x7f;        // pin 0 = Vdd, pin 7 = Gnd, rest turns on pull-ups  (0111 1111)  
  delay_us(5);       
  motor_grip     = CONTROLLER_IN.1 == 0 ? DOWN : motor_grip;
  motor_elbow    = CONTROLLER_IN.2 == 0 ? UP   : motor_elbow; 
  motor_base     = CONTROLLER_IN.3 == 0 ? UP   : motor_base; 
  motor_shoulder = CONTROLLER_IN.5 == 0 ? DOWN : motor_shoulder; 
  motor_wrist    = CONTROLLER_IN.6 == 0 ? DOWN : motor_wrist; 
        
  if(motor_grip == 1 || motor_elbow == 1|| motor_base == 1|| motor_shoulder == 1|| motor_wrist == 1) motor_up = 1;
  else motor_up = 0;
  
  if(motor_grip == 0 || motor_elbow == 0 || motor_base == 0 || motor_shoulder == 0 || motor_wrist == 0 ) motor_down = 1;
  else motor_down = 0;
} 

/* output to appropriate motors */                 
void output(void)
{             
	if(motor_grip == UP)
	{ 	GRIPPER_UP_SWITCH = ON;  GRIPPER_DOWN_SWITCH = OFF; }
	else if(motor_grip == DOWN)
	{  	GRIPPER_UP_SWITCH = OFF; GRIPPER_DOWN_SWITCH = ON; }

	if(motor_elbow == UP)
	{ 	ELBOW_UP_SWITCH = ON;  ELBOW_DOWN_SWITCH = OFF; }
	else if(motor_elbow == DOWN)
	{  	ELBOW_UP_SWITCH = OFF; ELBOW_DOWN_SWITCH = ON; }
	
	if(motor_base == UP)
	{ 	BASE_UP_SWITCH = ON;  BASE_DOWN_SWITCH = OFF; }
	else if(motor_base == DOWN)
	{  	BASE_UP_SWITCH = OFF; BASE_DOWN_SWITCH = ON; }
	
	if(motor_shoulder == UP)
	{ 	SHOULDER_UP_SWITCH = ON;  SHOULDER_DOWN_SWITCH = OFF; }
	else if(motor_shoulder == DOWN)
	{  	SHOULDER_UP_SWITCH = OFF; SHOULDER_DOWN_SWITCH = ON; }
	                     
	if(motor_wrist == UP)
	{ 	WRIST_UP_SWITCH = ON;	WRIST_DOWN_SWITCH = OFF; }
	else if(motor_wrist == DOWN)
	{  	WRIST_UP_SWITCH = OFF; WRIST_DOWN_SWITCH = ON; }    
    
   delay_ms(80);  	// pause for the motor to be turned on for sometimes before returning
}
               
/*** conversion function ***/
int performConversion(int motor_number)
{
    switch(motor_number)
    {
     	case GRIPPER:  	ADMUX = 0x00;  break;// select bit 0 to convert
     	case ELBOW:  	ADMUX = 0x01;  break;// select bit 1 to convert
     	case SHOULDER: 	ADMUX = 0x02;  break;// select bit 2 to convert     	     	   
    }
    ADCSR.6 = 1;   // start conversion   
 	
    while(converted != 1);   // spin until conversion is done
	  
  // read in value
    converted = 0;
    voltage = ADCL;
    upper_volt = ADCH;
    upper_volt = upper_volt << 8;
    voltage = upper_volt | voltage;
    return voltage;    
}


/* record final position from the potentiometers */
void recordFinal()
{             
    end_elbow_volt = performConversion(ELBOW);                      
    end_gripper_volt = performConversion(GRIPPER);
    end_shoulder_volt = performConversion(SHOULDER);


    printf("end elbow voltage is %d \r\n", end_elbow_volt);    
    printf("end gripper voltage is %d \r\n", end_gripper_volt);
    printf("end shoulder voltage is %d \r\n", end_shoulder_volt);
}
     

/* recording of input sensors function */
void record(void)
{ 
  /************* SHOULDER, ELBOW, GRIPPER SECTION *************************
      *********************************************************************                         
      *********************************************************************/
      if(first_time == TRUE)           
      {            
	    init_shoulder_volt = performConversion(SHOULDER);	 
        init_elbow_volt = performConversion(ELBOW);	    
        init_gripper_volt = performConversion(GRIPPER);	    
	    printf("init gripper voltage is %d \r\n", init_gripper_volt);
	    printf("init elbow voltage is %d \r\n", init_elbow_volt);   
	    printf("init shoulder voltage is %d \r\n", init_shoulder_volt);
      }	
                                                    
    /**************************   WRIST SECTION    ****************************                    
	 ****************************************    *****************************                    
	 ******************************************    ***************************                    
	 *********************************************************************/                    
	// first time round, just record that initial values                                      
	 if(first_time == TRUE)
	 {
		wrist_curr_inputL = WRIST_SENSOR_L;
		wrist_curr_inputR = WRIST_SENSOR_R;
		wrist_pulse = 0;	     	  
	 }       
	 // other times, check direction & increment/decrement pulse count appropriately
	 else
	 {                      
	 	//save old input
	 	wrist_old_inputL = wrist_curr_inputL;
	 	wrist_old_inputR = wrist_curr_inputR;
	                                                              
	  	// read in the current input
	 	wrist_curr_inputL = WRIST_SENSOR_L;
        wrist_curr_inputR = WRIST_SENSOR_R;                                                      
	 	          
	 	/* set the direction */  
	 	// case where both inputs were off or on
	 	if( (wrist_old_inputL == 0 && wrist_old_inputR == 0) ||
	 	     (wrist_old_inputL == 1 && wrist_old_inputR == 1) )
	 	  {
	 	  
	 	      if(wrist_curr_inputL != wrist_old_inputL)
		      {
		      	wrist_dir = RIGHT;
 		       	printf("going right \r\n");
 		      }
	 	      else  if(wrist_curr_inputR != wrist_old_inputR)
	 	      {
	 	         wrist_dir = LEFT;
		 		 printf("going left \r\n");
	 	      }       	 
	 	      else
	 	      	wrist_dir = STATIONARY;	  
	 	   }          
	 	   	 	 
	 	  // case where one of them is on
	 	  else 
	 	  {
	 	     if(wrist_curr_inputL != wrist_old_inputL)
		     {
		      	wrist_dir = LEFT;
 		       	printf("going left \r\n");
 		     }
	 	     else  if(wrist_curr_inputR != wrist_old_inputR)
	 	     {
	 	        wrist_dir = RIGHT;
	 	       	printf("going right \r\n");
	 	      }     
	 	      else
      	    	wrist_dir = STATIONARY;  	 	     
	 	   }      
	 	  
	      	// inc/dec the pulses
	  	if(wrist_dir == RIGHT && wrist_curr_inputR != wrist_old_inputR)
	 	{
	 	   	wrist_pulse+=2;
	 	   	 printf("pulse count is %d\r\n", wrist_pulse);	      	 	 	 	 
	 	}
	 	else if(wrist_dir == LEFT && wrist_curr_inputL != wrist_old_inputL)   
	 	{	 	
		 	wrist_pulse-=2;
		 	 printf("pulse count is %d\r\n", wrist_pulse);	      	 	 	 	 
		}
		 
		 temp_wrist_pulse = wrist_pulse;	
	 	
	 }
     
    /**************************   BASE SECTION    ****************************                    
	 ****************************************    *****************************                    
	 ******************************************    ***************************                    
	 *********************************************************************/                    
	// first time round, just record that initial values                                      
	 if(first_time == TRUE)
	 {
		first_time = FALSE;  
		base_curr_inputL = BASE_SENSOR_L;
		base_curr_inputR = BASE_SENSOR_R;
		base_pulse = 0;	     
	 }       
	 // other times, check direction & increment/decrement pulse count appropriately
	 else
	 {                      
	 	//save old input
	 	base_old_inputL = base_curr_inputL;
	 	base_old_inputR = base_curr_inputR;
	                                                              
	  	// read in the current input
	 	base_curr_inputL = BASE_SENSOR_L;
        base_curr_inputR = BASE_SENSOR_R;                                                      
	 	          
	 	/* set the direction */  
	 	// case where both inputs were off or on
	 	if( (base_old_inputL == 0 && base_old_inputR == 0) ||
	 	     (base_old_inputL == 1 && base_old_inputR == 1) )
	 	  {
	 	  
	 	      if(base_curr_inputL != base_old_inputL)
		      {
		      	base_dir = RIGHT;
 		       	printf("going right\r\n");
 		      }
	 	      else  if(base_curr_inputR != base_old_inputR)
	 	      {
	 	         base_dir = LEFT;
	 		 printf("going left \r\n");
	 	      }       	 
	 	      else
	 	      	base_dir = STATIONARY;	  
	 	   }          
	 	   	 	 
	 	  // case where one of them is on
	 	  else 
	 	  {
	 	     if(base_curr_inputL != base_old_inputL)
		     {
		      	base_dir = LEFT;
 		       	printf("going left \r\n");
 		     }
	 	     else  if(base_curr_inputR != base_old_inputR)
	 	     {
	 	       	base_dir = RIGHT;
	 	       	printf("going right \r\n");
	 	      }     
	 	      else
      	    	base_dir = STATIONARY;  	 	     
	 	   }      
	 	  
	      	// inc/dec the pulses
	  	if(base_dir == RIGHT && base_curr_inputR != base_old_inputR)
	 	{
	 	   	base_pulse++;
	 	   	 printf("pulse count is %d\r\n", base_pulse);	      	 	 	 	 
	 	}
	 	else if(base_dir == LEFT && base_curr_inputL != base_old_inputL)   
	 	{	 	
		 	base_pulse--;
		 	 printf("pulse count is %d\r\n", base_pulse);	      	 	 	 	 
		}
		 
		 temp_base_pulse = base_pulse;	
	 	
	 }
}
   

/* replay the data */
void play(void)
{     
 	  char temp_base_old_input;
	  char temp_base_curr_input; 
     	  char temp_wrist_old_input;
	  char temp_wrist_curr_input;      
	  int temp_volt;

	  
  	 /************* SHOULDER SECTION ******************
	  ***********************************************
	  ***********************************************/
	  /*
	  while(shoulder_done != 1)
	  {
	  	temp_volt = performConversion(SHOULDER);	  
	 	 printf("temp_volt = %d, init_shoulder_volt = %d\r\n", temp_volt, init_shoulder_volt);
	 	 if(temp_volt > init_shoulder_volt && ( (temp_volt - init_shoulder_volt) > 10) ) 
	  	{          	                             
			motor_shoulder = DOWN;
			output();	    
	  	}      
	  	else if(temp_volt < init_shoulder_volt && ( (init_shoulder_volt - temp_volt) > 10))
	  	{
	  		motor_shoulder = UP;
	  		output(); 	 
	 	} 
	  	else
	  	 shoulder_done = 1;
	  }*/ 
	  
	 /******************BASE SECTION    ******************
	  ****************************************************
	  ****************************************************/  
	  while(base_done != 1)
	  {
	  	printf("temp_base_pulse %d\r\n", temp_base_pulse);
	  	  
     	 // for base
    	  if(temp_base_pulse > 0)
     	 {
       		temp_base_pulse--;
       		motor_base = UP;     
      
	  	   	temp_base_curr_input = BASE_SENSOR_R;
			temp_base_old_input = temp_base_curr_input; 
		
			while(temp_base_curr_input != (~temp_base_old_input & 0x01))
       		{
    	   	 	//output();
   	   	 		BASE_UP_SWITCH = ON; BASE_DOWN_SWITCH = OFF; 
  				delay_ms(60);   		
       		 	temp_base_curr_input = BASE_SENSOR_R;  
    	   	}
 	     }             
    	 else if(temp_base_pulse < 0)
   		 {
   	    	temp_base_pulse++;
    	   	motor_base = DOWN;
  	     
        	temp_base_curr_input = BASE_SENSOR_L;
			temp_base_old_input = temp_base_curr_input; 
		
			while(temp_base_curr_input != (~temp_base_old_input & 0x01))
       		{
	       	 	//	output();
	       	 	BASE_UP_SWITCH = OFF; BASE_DOWN_SWITCH = ON;    
  				delay_ms(60);  
       	 		temp_base_curr_input = BASE_SENSOR_L;
       		}
     	 } 
     	 else  
      	{   
   			BASE_UP_SWITCH = OFF; BASE_DOWN_SWITCH = OFF; 
       		motor_base = IDLE;
       		base_done = 1;
 			printf("base_done\r\n");
      	}   
      
	  }       

 
	    
	 /************* ELBOW SECTION ******************
	  ***********************************************
	  ***********************************************/
	  
	  while(elbow_done != 1)
	  {	
		  temp_volt = performConversion(ELBOW);	  
		  printf("temp_volt = %d, init_elbow_volt = %d\r\n", temp_volt, init_elbow_volt);
		  if(temp_volt > init_elbow_volt && ( (temp_volt - init_elbow_volt) > 3) ) 
		  {          	                             
				motor_elbow = DOWN;
				ELBOW_UP_SWITCH = OFF; ELBOW_DOWN_SWITCH = ON; 		
		  }      
		  else if(temp_volt < init_elbow_volt && ( (init_elbow_volt - temp_volt) > 3))
		  {
		  		motor_elbow = UP;
	  			ELBOW_UP_SWITCH = ON; ELBOW_DOWN_SWITCH = OFF; 
		  } 
		  else 
		  {       
           ELBOW_UP_SWITCH = OFF; ELBOW_DOWN_SWITCH = OFF; 
		   motor_elbow = IDLE;
		   elbow_done = 1;  
           printf("elbow_done\r\n");
		  }
      
	  }
	  
	  

      
	  /************ WRIST SECTION *********************
	   ************************************************
	   ************************************************/
	  
	  while(wrist_done != 1)
	  {
	  	printf("temp_wrist_pulse %d\r\n", temp_wrist_pulse);	  	        
      	if(temp_wrist_pulse > 0)
      	{
       		temp_wrist_pulse--;
       		motor_wrist = UP;     
      
      		temp_wrist_curr_input = WRIST_SENSOR_R;
			temp_wrist_old_input = temp_wrist_curr_input; 
		
			while(temp_wrist_curr_input != (~temp_wrist_old_input & 0x01))
       		{    
   				WRIST_UP_SWITCH = ON; WRIST_DOWN_SWITCH = OFF;  
   				delay_ms(60);   
       		 	temp_wrist_curr_input = WRIST_SENSOR_R;  
       		}
    	}	             
     	else if( temp_wrist_pulse  < 0)
  	    {
    	   	temp_wrist_pulse++;
       		motor_wrist = DOWN;
       
    	    temp_wrist_curr_input = WRIST_SENSOR_L;
			temp_wrist_old_input = temp_wrist_curr_input; 
		
			while(temp_wrist_curr_input != (~temp_wrist_old_input & 0x01))
       		{    
   				WRIST_UP_SWITCH = OFF; WRIST_DOWN_SWITCH = ON;    
   				delay_ms(60);
       	 		temp_wrist_curr_input = WRIST_SENSOR_L;
       		}  	
     	 } 
         else
     	 {
     	 	WRIST_UP_SWITCH = OFF; WRIST_DOWN_SWITCH = OFF;   
	  		motor_wrist = IDLE;	       
       		wrist_done = 1;       
	      	printf("wrist_done\r\n");
     	 }           
     	 
      }
       
               
  
	 
      	  
	 /************* GRIPPER SECTION ******************
	  ***********************************************
	  ***********************************************/
	  
      while(gripper_done != 1)
      {
	  	temp_volt = performConversion(GRIPPER);	  
	  	printf("temp_volt = %d, init_gripper_volt = %d\r\n", temp_volt, init_gripper_volt);
	  	if(temp_volt > init_gripper_volt && ( (temp_volt - init_gripper_volt) > 10) ) 
	  	{          	                             
			motor_grip = UP;
			GRIPPER_UP_SWITCH = ON; GRIPPER_DOWN_SWITCH = OFF;
		  }      
		  else if(temp_volt < init_gripper_volt && ( (init_gripper_volt - temp_volt) > 10))
		  {
		  		motor_grip = DOWN;
	  			GRIPPER_UP_SWITCH = OFF; GRIPPER_DOWN_SWITCH = ON;
		  } 
		  else
		  {                                                  
		   	GRIPPER_UP_SWITCH = OFF; GRIPPER_DOWN_SWITCH = OFF; 
		    motor_grip = IDLE;
		    gripper_done = 1;                    
	    	printf("gripper_done\r\n");
   		  }
      }	
    		       
       /** revert back to normal mode **/
         mode = NORMAL;
      
}            



/* replay the data */
void playForward(void)
{     
 	  char temp_base_old_input;
	  char temp_base_curr_input; 
      char temp_wrist_old_input;
	  char temp_wrist_curr_input;      
	  int temp_volt;

	  
  	 /************* SHOULDER SECTION ******************
	  ***********************************************
	  ***********************************************/
	   
	  /*
	  while(shoulder_done != 1)
	  {
	 	 temp_volt = performConversion(SHOULDER);	  
	  	printf("temp_volt = %d, init_shoulder_volt = %d\r\n", temp_volt, init_shoulder_volt);
	  	if(temp_volt > init_shoulder_volt && ( (temp_volt - init_shoulder_volt) > 10) ) 
	  	{	          	                             
			motor_shoulder = DOWN;
			output();	    
	  	}      
	  	else if(temp_volt < init_shoulder_volt && ( (init_shoulder_volt - temp_volt) > 10))
	  	{
	  		motor_shoulder = UP;
	  		output(); 	 
	 	 } 
	  	else
	  	 shoulder_done = 1;
	  } */
	   
	 /******************BASE SECTION    ******************
	  ****************************************************
	  ****************************************************/  
	  while(base_done != 1)
	  {
	  	printf("temp_base_pulse %d\r\n", temp_base_pulse);
	  	  
     	 // for base
    	  if(temp_base_pulse > 0)
     	 {
       		temp_base_pulse--;
       		motor_base = DOWN;     
      
	  	   	temp_base_curr_input = BASE_SENSOR_R;
			temp_base_old_input = temp_base_curr_input; 
		
			while(temp_base_curr_input != (~temp_base_old_input & 0x01))
       		{
    	   	 
   	   	 		BASE_UP_SWITCH = OFF; BASE_DOWN_SWITCH = ON; 
  				delay_ms(60);  // give motor sometimes to settle
       		 	temp_base_curr_input = BASE_SENSOR_R;  
    	   	}
 	     }             
    	 else if(temp_base_pulse < 0)
   		 {
   	    	temp_base_pulse++;
    	   	motor_base = UP;
  	     
        	temp_base_curr_input = BASE_SENSOR_L;
			temp_base_old_input = temp_base_curr_input; 
		
			while(temp_base_curr_input != (~temp_base_old_input & 0x01))
       		{
	       	 	BASE_UP_SWITCH = ON; BASE_DOWN_SWITCH = OFF;       
	       	 	delay_ms(60);  
       	 		temp_base_curr_input = BASE_SENSOR_L;
       		}
     	 } 
     	 else  
      	{   
   			BASE_UP_SWITCH = OFF; BASE_DOWN_SWITCH = OFF; 
       		motor_base = IDLE;
       		base_done = 1;
 			printf("base_done\r\n");
      	}   
      
	  }       

 
	    
	 /************* ELBOW SECTION ******************
	  ***********************************************
	  ***********************************************/
	  
	  while(elbow_done != 1)
	  {	
		  temp_volt = performConversion(ELBOW);	  
		  printf("temp_volt = %d, end_elbow_volt = %d\r\n", temp_volt, end_elbow_volt);
		  if(temp_volt > end_elbow_volt && ( (temp_volt - end_elbow_volt) > 3) ) 
		  {          	                             
				motor_elbow = DOWN;
				ELBOW_UP_SWITCH = OFF; ELBOW_DOWN_SWITCH = ON; 			
		  }      
		  else if(temp_volt < end_elbow_volt && ( (end_elbow_volt - temp_volt) > 3))
		  {
		  		motor_elbow = UP;
	  			ELBOW_UP_SWITCH = ON; ELBOW_DOWN_SWITCH = OFF; 
		  } 
		  else 
		  {       
           ELBOW_UP_SWITCH = OFF; ELBOW_DOWN_SWITCH = OFF; 
		   motor_elbow = IDLE;
		   elbow_done = 1;  
           printf("elbow_done\r\n");
		  }
      
	  }
	  
	  

      
	  /************ WRIST SECTION *********************
	   ************************************************
	   ************************************************/
	  
	  while(wrist_done != 1)
	  {
	  	printf("temp_wrist_pulse %d\r\n", temp_wrist_pulse);	  	        
      	if(temp_wrist_pulse > 0)
      	{
       		temp_wrist_pulse--;
       		motor_wrist = DOWN;     
      
      		temp_wrist_curr_input = WRIST_SENSOR_L;
			temp_wrist_old_input = temp_wrist_curr_input; 
		
			while(temp_wrist_curr_input != (~temp_wrist_old_input & 0x01))
       		{    
   				WRIST_UP_SWITCH = OFF; WRIST_DOWN_SWITCH = ON;  
  				delay_ms(60);   
       		 	//output();
       		 	temp_wrist_curr_input = WRIST_SENSOR_L;  
       		}
    	}	             
     	else if( temp_wrist_pulse  < 0)
  	    {
    	   	temp_wrist_pulse++;
       		motor_wrist = UP;
       
    	    temp_wrist_curr_input = WRIST_SENSOR_R;
			temp_wrist_old_input = temp_wrist_curr_input; 
		
			while(temp_wrist_curr_input != (~temp_wrist_old_input & 0x01))
       		{    
   				WRIST_UP_SWITCH = ON; WRIST_DOWN_SWITCH = OFF;  
  				delay_ms(60);  
       	 		//output();
       	 		temp_wrist_curr_input = WRIST_SENSOR_R;
       		}  	
     	 } 
         else
     	 {
     	 	WRIST_UP_SWITCH = OFF; WRIST_DOWN_SWITCH = OFF;   
	  		motor_wrist = IDLE;	       
       		wrist_done = 1;       
	      	printf("wrist_done\r\n");
     	 }           
     	 
      }
       
                    	  
	 /************* GRIPPER SECTION ******************
	  ***********************************************
	  ***********************************************/
	  
      while(gripper_done != 1)
      {
	  	temp_volt = performConversion(GRIPPER);	  
	  	printf("temp_volt = %d, end_gripper_volt = %d\r\n", temp_volt, end_gripper_volt);
	  	if(temp_volt > end_gripper_volt && ( (temp_volt - end_gripper_volt) > 10) ) 
	  	{          	                             
			motor_grip = UP;
			GRIPPER_UP_SWITCH = ON; GRIPPER_DOWN_SWITCH = OFF;
			output();	    
		  }      
		  else if(temp_volt < end_gripper_volt && ( (end_gripper_volt - temp_volt) > 10))
		  {
		  		motor_grip = DOWN;
	  			GRIPPER_UP_SWITCH = OFF; GRIPPER_DOWN_SWITCH = ON;
		  } 
		  else
		  {                                                  
		   	GRIPPER_UP_SWITCH = OFF; GRIPPER_DOWN_SWITCH = OFF; 
		    motor_grip = IDLE;
		    gripper_done = 1;                    
	    	printf("gripper_done\r\n");
   		  }
      }	
    	
	       
       /** revert back to normal mode **/
       mode = NORMAL;
      
}


/* initialize everything */  
void initialize(void) 
{             
  /* setup the serial UART */
  UCR = 0x10 + 0x08 ;
  UBRR = 25 ;        
 
  /* Init port B to be output for bit 0 and 7, rest as input (1000 0001)*/
  /* PORT B for connection to controller */
  CONTROLLER_DIR = 0x81;    
  
  /* Init motor switches to be output */
  MOTOR_DIR            = 0xff;
  MOTOR                = 0xff;        
  GRIPPER_UP_DIR       = 1;     
  GRIPPER_DOWN_DIR     = 1;
  GRIPPER_UP_SWITCH    = 1;
  GRIPPER_DOWN_SWITCH  = 1;    
  DDRA.5 = 1;
  DDRA.6 = 1;
  DDRA.7 = 1;
  PORTA = PORTA | 0xe0;	

  /* init the sensors port to be input */           
  DDRD = DDRD & 0xc0;
  
  /* Init portA to be input */
  DDRA = DDRA & 0xf8;
  PORTA = PORTA &0xf8;

  /* setup the timer 0 */     
  /* 62.5 x (64 x0.25) microSec = 1.0 mSec, so prescale 64, and count 62 times. */ 
  timer0_reload = 256-62;		// value for 1 ms    
  TCNT0 = timer0_reload;		// preload timer 0 so that it interrupts after 1 ms  
  TCCR0 = 3;			   	// prescalar to 64    
  TIMSK = 1;			   	// turn on timer 0 overflow ISR  

  /* interval timing stuff */
  scan_time = SCAN_INTERVAL;

  /* motors stuff */
  motor_grip     = IDLE;      
  motor_elbow    = IDLE;     
  motor_base     = IDLE;    
  motor_shoulder = IDLE;     
  motor_wrist    = IDLE;      
  motor_up       = IDLE;
  motor_down     = IDLE;
   
  // base photosensor stuff
  first_time = TRUE;
  
  base_old_inputL = 0;
  base_old_inputR = 0;
  base_curr_inputL = 0;
  base_curr_inputR = 0;  
  base_pulse = 0;
  base_dir = STATIONARY;    
  
  wrist_pulse = 0;
  wrist_dir = STATIONARY;    
  
  // potentiometer stuff
  // set ADC enable (bit 7), single conversion mode, set interrupt flag enable, prescale 64
  ADCSR = 0x8e;
    
  // control operation mode stuff
  mode = NORMAL;
  old_mode = NORMAL;
  replay = 0;
  training = 0;
  
  printf("initialized\r\n");
  #asm 
  sei 
  #endasm
}