/* behavior box program */

/*--------------------------------------------------------------------------*/
/* The following variables are defined to ease the calibration of our robot. */
/* The numbers or functions on the right take the place of the variables named on */
/* left.*/

#define Lmotor 1  
#define Lenco 0
#define Lhat analog(2)
#define left_drift -1

#define Rmotor 3
#define Renco 1
#define Rhat analog(3)
#define right_drift 2

#define tape_value 50 /* average tape value for top hats */
#define low_tape 90  /* value of top hat near white edge of tape */
#define hi_tape 110  /* value of top hat near black edge of tape */

/*-------------------------------------------------------------------------*/
/* the following global variables are used in the calibration process and set the 
   number encoder readings for turning and distance.*/

int encl,encr,i;
int inch=10;/* number of ticks per foot travel */
int spin=47; /* number of ticks per 90 degrees */
int drive=79; /* distance in inches to next box */
int miss=0; /* value used incase the robot misses the box upon arrival */
int stint=0; /* value used to indicate what leg of the course the robot is on */
int left_mem; /* values used to hold the condition of the top hat sensors */
int right_mem;

/*--------------------------------------------------------------------------*/
/*  main function 
    once the handyboard is turned on it will reset the encoders and ask the user 
    if they would like to calibrate the system. */

void main()
{
    int too_far;
    
    
    enable_encoder(Lenco);
    enable_encoder(Renco);
    
    printf("press start to  calibrate\n");
    sleep(3.0);
    if(start_button())calibrate();
    
    printf("Ready to GO\n");
    while(!start_button());  /* After calibration the robot is ready to proceed */
   
    too_far=drive*inch;  /* setting the distance to the next box */
       
    for(stint=0;stint < 12;stint++)
      {
        /* robot will enter a 3 lap loop moving from box to box */
        
        miss=0;    /* resets global constants for each loop */
        left_mem=0;
        right_mem=0;
        
        stage();  /* robot will stage itself on the black line for accurate 
			projection to the next square. This is done in case the robot 
			is placed in an incorrect starting position. */

        go(drive-12,100);  /* Based on the calibration or defalt values the 
					robot will move a specified distance that is 
					shorter than the distance to the next box. 
					During its travel to the next box only the 
					encoders are actively sensing environmental 
					changes.*/
        
        while(1)
          {
            straight(20);  /* When the robot nears the next box it will start 
					searching for the black tape while it is moving 
					forward */

            if(Lhat>tape_value)break; /* if the robot senses the black tape it 							will */
            else if(Rhat>tape_value)break; /* stop and exit the loop. If no 
								black tape is */
              else if(average_encoder()>drive*inch)   /* sensed it increments 
									the miss value and 
									exits*/
                  {                                   
                    beep();                           
                    miss=1;
                    break;
                }
        }
        
        brake(1);  /* motor command to brake the robot */
        
        /* once the robot exits the above loop it will now use sensor 
		information to evaluate its orientation near the box. If the robot 
		has gone the complete distance to the box and has not come across 
		a black line it will begin a search function (find_box()) to look 
		for tape.
           If the robot has come across tape it will use the condition() 
		function to find what direction the box is in front of the robot.		  */
        
        if(miss==1)find_box(); 
        else condition();
        printf("stint %d\n",stint);
        
    }
    printf("all done\n");  /* print command to tell that the robot has 
					completed the run or is lost */
}


void reset_enco()  /* function to reset encoders */
{
    reset_encoder(Lenco);
    reset_encoder(Renco);
}

int average_encoder() /* function to find the average distance the robot has 
				driven */
{
    int a;
    a=(read_encoder(Lenco)+read_encoder(Renco))/2;
    return(a);
}

int move(int a,int b)  /* simple motor function takes two speed values for 
				each motor */ 
{
    motor(Lmotor,a);
    motor(Rmotor,b);
}

int brake(int x)   /* A series of brake commands to stop the robot from 
				drifting after the motors are turned off. The momentum 
				of the robot will carry it a few extra inches. */ 
{
    if (x==1)move(-10,-10);
    else if (x==0) move(20,-20);
      else if (x==2) move(20,20);
        sleep(0.1);
    ao();
}

int go(int travel,int speed) 
		/* This function will make the robot drive a specified distance.
              it calls the straight() function for motor control while it 
              makes sure the average encoder ticks are less than the specified 
              distance to travel. travel accepts distance in encoder ticks*/
{
    int position;
    position=average_encoder()+(inch*travel);
    
    while(average_encoder() < position)
      {
        straight(speed);
    } 
    if(speed<0)brake(2);
    else brake(1);
    
}

void straight(int velocity) 
	/* this motor control function will adjust the speed of both motors
         depending on the encoder readings form each side of the robot.
         It keeps the robot driving straight */
{
    int drift;
    
    drift=read_encoder(Lenco)-read_encoder(Renco); /* takes the difference 
								    from the encoder values */
    
    if (drift < left_drift)
      move(velocity,0);
    else if (drift > right_drift)
        move(0,velocity);
      else move(velocity,velocity);
    
}


int turn(int degree) 
		/* uses a skid steering approach to change the robots vector
               it will drive the right motor forward for a specific number of 
               turns using the encoders and drive the left motor backward 
               for the same number of turns. This makes the robot turn like a 
               Tank. */
{
    int drift,ticks;
    
    reset_enco();
    
    ticks=degree/90*spin; /* sets the number of ticks for the spin */
    
    /*turns on motors in opposite direction 
	until the number of ticks has been reached*/
    
    while(average_encoder() < ticks)  
      {
        drift=read_encoder(Lenco)-read_encoder(Renco);
        
        if (drift < -5)
          move(-50,10);
        else if (drift > 5)
            move(-10,50);
          else move(-50,50);
        
    }
    brake(0);
    reset_enco();
}

void stage()  /* The robot must be staged on the edge of a box to ensure
                 perpendicluar allignment to drive to the next box.*/ 
{
    int l,r,bump;
    l=0;
    r=0;
    bump=0;
    
    /* the robot reads the left and right top hat sensor values and will try 
	to stop the robot where they are both reading very similar values. */
    
    while(bump<1000)
      {
        if(l==1 && r==1)
          {
            ao();
            reset_enco();
            break;
        }
        l=left_hat();
        r=right_hat();
        bump=bump+1;    /* an counter was added to avoid being stuck in an 
					infinite loop */
    }
    
}

int right_hat()  /* function for the right top hat sensor used in the stage 
			  function to control the right side of the robot and align 
			  the right sensor */
{
    if(Rhat > low_tape && Rhat < hi_tape)return(1);
    else if(Rhat < low_tape)
        {
          motor(Rmotor,7);
          return(0);
      }
      else if(Rhat > hi_tape)
          {
            motor(Rmotor,-7);
            return(0);
        }
}

int left_hat() /* same function as above except for the left side */
{
    if(Lhat > low_tape && Lhat < hi_tape)return(1);
    else if(Lhat < low_tape)
        {
          motor(Lmotor,7);
          return(0);
      }
      else if(Lhat > hi_tape)
          {
            motor(Lmotor,-7);
            return(0);
        }
}
void condition()  /* When the robot nears the next box and begins its search 
			   for the tape it will stop if the tape is found. The 
			   values for sensors are recorded and stored in memory. The 
			   conditions below assume the robot is making laps
                     in a counter clockwise direction */
{
    if(Lhat>tape_value)left_mem=1;
    if(Rhat>tape_value)right_mem=1;
    
    go(1,100); /* the robot moves forward one more inch and stops */
    
    /* the sensor values are recorded again and added to the 
	previous sensor values */
    
    if(Lhat>tape_value)left_mem=left_mem+1;  
    if(Rhat>tape_value)right_mem=right_mem+1;
    
    /* based on the sensor input the robot can tell where it is 
	in relation to the box infront */
    
    if(left_mem>0 && right_mem==0) 
	/* if the left sensor read the line but the right did not 
		the box is in front and to the left of the robot */
      {
        go(6,100);
        turn(90);
        go(3,20);
    }
    else if(right_mem>0 && left_mem==0)  
	/* if the right sensor read the line but the left did not 
		the box is in front and to the right */
        {
          go(6,100);
          turn(90);
          go(4,-20);                    
      }
      else   /* otherwise both sensors read the line and the box is directly 
			in front of the robot */
        {
          go(6,100);
          turn(90);
      }
    
}

void find_box()  
	/* if the robot did not sense any line it continued until the total 
		distance was reached then the miss counter was incremented */
{
    int guess_miss,dir;
    
    /* The robot will first assume the box is on its left because our robot 
	 has a slight drift to the right. It will set a distance to travel and 
	 look for the tape */

    turn(90);          
    dir=1;
    guess_miss=12*inch;
    
    while(1)
      {
        printf("miss %d\n",miss);
        if(dir==1)straight(20);
        else straight(-20);
        
        if(average_encoder() > guess_miss)  
		/* if no tape is sensed after the guess travel it will stop
		   increment the miss counter and reverse direction for twice 
               the previous distance. */
          {
            beep();
            dir=0;
            guess_miss=average_encoder()+2*guess_miss;
            miss=miss+1;
            if(miss > 2)break; /* if the it has searched more than two trips 
						the loop is exited */
        }
        else if(Rhat > tape_value)break; /* if the box is sensed the robot 
								resumes operation */
          else if(Lhat > tape_value)break;
    }
    
    if(miss > 2) /* after two trips the robot will assume it cannot find the 
				box and stop */
      {
        ao();
        printf("I can't find thebox\n");
        sleep(2.0);
        stint=100;
    }
    else if (miss==1)go(6,100); 
	/* If the robot assumed right the first time and the box is on the 
	   left it will move into the box and resume normal operation. */
      else if (miss==2)   
		/* If the robot missed the first try and found the box on the 
		   second try the box must have been on the right side, the robot 
		   backs into position and resumes normal operation. */
          {
            beep();
            beep();
            go(4,-20);
        }
}

void calibrate() /* calibration function used by the operator in the first 
			  moments of power-up to set the total distance between the 
			  boxes and the number of encoder ticks for a 90 degree turn 
		     */
{
    printf("calibrate distance\n");
    sleep(3.0);
    printf("press STOP to  end\n");
    sleep(3.0);
    
    while(!stop_button())
      {
        drive=knob();
        printf("dist is %d\n",knob());
        sleep(0.2);
    }
    
    printf("calibrate turn\n");
    sleep(3.0);
    printf("press STOP to  end\n");
    sleep(3.0);
    
    while(!stop_button())
      {
        spin=knob();
        printf("spin is %d\n",knob());
        sleep(0.2);
    }
    printf("press START for test spin");
    while(!start_button());
    turn(90);
    
}