#use "cmucamlib.ic"

/**************************************************************************
Intro to intellegent robotics
Dr. Hougen
Spring 2004
Project 1
Team 5
***********************************************************************/


int TICKS_PER_LEFT = 96;
int TICKS_PER_RIGHT = 101;
int TICKS_PER_TURNAROUND=185;
float TIME_TO_SLEEP = 1.5;
int TICKS_TO_CAMERA = 65;
int TICKS_TO_COLOR = 85;
int R_MOTOR = 3;
int L_MOTOR = 0;
int R_SENSOR = 14;
int L_SENSOR = 15;

//goFast() moves the robot at a reasonably fast pace
void goFast()
{
    motor(L_MOTOR,-50);
    motor(R_MOTOR,-48);
}

//goSlow() moves the robot half the speed of goFast()
void goSlow()
{
    motor(L_MOTOR,-25);
    motor(R_MOTOR,-24);
}

//forward(int x) moves the robot x encoder ticks
void forward(int x)
{
    reset_encoder(0);
    reset_encoder(1);
    
    motor(L_MOTOR,-50);
    motor(R_MOTOR,-48);
    
    
    while(1)
      {
        
        if(averageEncoders() >= x)
          {
            stop();
            reset_encoder(0);
            reset_encoder(1);
            return;
        }
      }
    return;
}//end foward(int x)


//findBlack() runs while the robot is heading toward black tape
//  it will align the robot with the black tape and then return control
void findBlack()
{
    while(1)
      {
        
        if ((!digital(R_SENSOR)) && (digital(L_SENSOR)))
          { // when the right IR sensor is on the blacktape and the left sensor is not then,
            // the right motor is moved backward and left in forward mode to get the robot aligned to the blacktape
            while(!((!digital(L_SENSOR))&&(!digital(R_SENSOR))))
              {
                motor(R_MOTOR,30); 
                motor(L_MOTOR,-35);
            }
            stop();
            return;
        }
        if ((!digital(L_SENSOR)) && (digital(R_SENSOR)))
          { //this is the opposite of the above
            while(!((!digital(L_SENSOR))&&(!digital(R_SENSOR))))
              {
                motor(L_MOTOR,30); 
                motor(R_MOTOR,-35);
            }
            stop();
            return;
        }
        if ((digital(L_SENSOR) && digital(R_SENSOR))) // both sensors see white
          {

        }
       if (!((digital(R_SENSOR)) || (digital(L_SENSOR)))) // both sensors see black
          {
            printf("Black found\n");
            return;
        }
    }
    
}

//backward() moves the robot backwards
void backward()
{
    motor(L_MOTOR,25);
    motor(R_MOTOR,24);
   
    return;
}

//turn off motors
void stop()
{
    off(L_MOTOR);
    off(R_MOTOR);
    return;
}

//diffEncoders() take the difference of the encoders
//  we had our right encoder as 0 and left as 1
int diffEncoders()
{
    return (int)(read_encoder(0)-read_encoder(1));
}

//averageEncoders() adds the encoder values and divides by 2
int averageEncoders()
{
    return (int)((read_encoder(0)+read_encoder(1))/2);
}

//left() turns the robot left 90 degrees
void left()
{
    reset_encoder(0);
    reset_encoder(1);
    motor(L_MOTOR,50);
    motor(R_MOTOR,-50);
    while(1)
      {
        if (averageEncoders() >= TICKS_PER_LEFT)
          {
            stop();
            return;
        }
        if (diffEncoders() > 0) //right has traveled farther
          {
            motor(L_MOTOR,40);
            motor(R_MOTOR,-50);
            
        }
        else if (diffEncoders() < 0) //left has traveled farther
            {
              motor(L_MOTOR,50);
              motor(R_MOTOR,-40);    
          }
          else  //the encoders are the same
            motor(L_MOTOR,50);
            motor(R_MOTOR,-50);
    }
}//end left

//right() turns the robot right 90 degrees
void right()
{
    reset_encoder(0);
    reset_encoder(1);
    motor(L_MOTOR,-50);
    motor(R_MOTOR,50);
    while(1)
      {
        
        if (averageEncoders() >= TICKS_PER_RIGHT)
          {
            stop();
            return;
        }
        if (diffEncoders() > 0) //right has traveled farther
          {
            motor(L_MOTOR,-50);
            motor(R_MOTOR,40);
            
        }
        else if (diffEncoders() < 0) //left has traveled farther
            {
              motor(L_MOTOR,-40);
              motor(R_MOTOR,50);    
          }
          else
            motor(L_MOTOR,-50);
            motor(R_MOTOR,50);
    }
}//end right()

//turnaround turns the robot right 180 degrees
void turnaround()
{
    reset_encoder(0);
    reset_encoder(1);
    motor(L_MOTOR,-50);
    motor(R_MOTOR,50);
    while(1)
      {
        
        if (averageEncoders() >= TICKS_PER_TURNAROUND)
          {
            stop();
            return;
        }
        if (diffEncoders() > 0) //right has traveled farther
          {
            motor(L_MOTOR,-50);
            motor(R_MOTOR,40);
            
        }
        else if (diffEncoders() < 0) //left has traveled farther
            {
              motor(L_MOTOR,-40);
              motor(R_MOTOR,50);    
          }
          else
            motor(L_MOTOR,-50);
        motor(R_MOTOR,50);
        
    }
}//end turnaround()

//all the track_color() methods return the confidence
//  that the camera sees that color

int track_pink()
{
    int crmin;
    if (trackRaw(100,180,12,23,12,20) > 0) {
        return track_confidence;
    } else return 0;
} // end track_pink() //

int track_green()
{
    int crmin;
    if (trackRaw(95,120,135,170,15,25) > 0) {
        return track_confidence;
    } else return 0;
} // end track_green() //

int track_yellow()
{
    int crmin;
    if (trackRaw(140,180,140,180,13,19) > 0) {
        return track_confidence;
    } else return 0;
} // end track_yellow() //

int track_orange2()
{
    int crmin;
    if (trackRaw(125,185,45,65,13,19) > 0) {
        return track_confidence;
    } else return 0;
} // end track_yellow() //

int track_blue2()
{
    int crmin;
    if (trackRaw(30,65,125,185,75,130) > 0) {
        return track_confidence;
    } else return 0;
} // end track_yellow() //


//clamp_camera_rgb() came from clamp_camera_yuv only changed the numbers
//  to set the camera in RGB mode
int clamp_camera_rgb()
{
    int i;
    printf("Point at white  and press start\n"); 
    while (!start_button());
    send_R_command_require_reply("CR 18 44\r",9);
    /*    for (i= 15; i > 0; i--) {
        printf("Setting white   balance %d..\n", i);
        sleep(1.0);
    }
*/  //we commented this out because the white balance was 
    //  taking too long in testing.  It worked fine without it.
    send_R_command_require_reply("CR 18 40\r",9);
    printf("Done\n");
} // clamp_camera_RGB() //


void main()
{
    int i;
    init_camera(); // initialize the camera 
    clamp_camera_rgb(); //clamp camera white balance (RGB mode)
    
    while (!start_button()) //starts moving after you press start
      {}
    
    
    enable_encoder(0);//enable encoders
    enable_encoder(1);
    sleep(1.0);
    
    //move until you find black tape
    goFast();
    findBlack();
    
    while (1)
      { 
        //move the camera to the center of the square
        forward(TICKS_TO_CAMERA);
        sleep(1.5);
        //look for a color
        if (track_orange2() > 0) //if orange is found
          {
            printf("orange found\n");
            forward(TICKS_TO_COLOR); //move the turning point on top of the color
            left(); 
            goSlow(); //start moving
            findBlack();//look for the black tape of the square you are leaving
            sleep(1.0);
        }
        if (track_blue2() > 0)// if blue is found
          {
            printf("blue found\n");
            forward(TICKS_TO_COLOR); //move the turning point on top of the color
            right();
            goSlow();  //start moving
            findBlack(); //look for the black tape of the square you are leaving
            sleep(1.0);
        }
        if (track_green() > 0) //if green is found
          {
            printf("green found\n");
            sleep(1.0);
        }
        
        if (track_pink() > 0) //if pink is found
          {
            printf("pink found\n");
            forward(TICKS_TO_COLOR); //move the turning point on top of the color
            turnaround();
            goSlow(); //start moving
            findBlack(); //look for the black tape of the square you are leaving
            sleep(1.0);
        }
        
        if (track_yellow() > 0) //if yellow is found
          {
            printf("yellow found\n");
            forward(TICKS_TO_CAMERA); //move the turning point on top of the color
            for (i=0;i<20;i++) //beeping course completed
              {
                beep();
            }
            stop();
            return;
        } 
        goFast(); // go to next square
        findBlack(); // look for the tape of the next square
    }
    return;
}