/////////////////////////////////////////////////////////////////
//  Project 1
//   Team 1
//
//  Code to make our robot ("Kirby") go in a straight line 
//  for six feet, turn 90 degrees, line up for the next box,
//  and then repeat until 3 circits are made.
/////////////////////////////////////////////////////////////////

#define thres 90           // set a threshold of reflectance to check against
#define left_enc 1    
#define right_enc 0
#define left_motor 2
#define right_motor 0
#define ticks_2_turn 34    // number of encoder counts to turn for
#define front_line analog(2)

int count = 0;            // boxes traveled to
int leftspeed = 80;       // initial motor speeds
int rightspeed = 80;
int enc0, enc1;
int resets = 0;           // resets * ticks_2_dist is the distance travelled
int ticks_2_dist=198;

void main()
{
    int col;
    
    alloff();
    printf( "Project1!\n" );
    
    // Wait for the start button to be hit, and allow
    // time for the user to get away
    while( !start_button()){
        printf("\n%d", knob());
    }
    ticks_2_dist=knob();
    sleep(2.0);
    
    getSet();
    printf("I am starting off!!");
    rampup();
    sleep(0.1);
    
    // start counting distance
    enable_encoder(right_enc);
    enable_encoder(left_enc);
    while(!stop_button())
      {
        enc0=read_encoder(right_enc);
        enc1=read_encoder(left_enc);
        printf("\n%d, %d ", enc0, enc1);
        
        // see if we've gone 6 feet yet...
        if(resets>=2)
          {
            count++;
            break_all_motors();
            sleep(0.2);
            if(count>=12)  // if we've made three laps, end this
              return;
            
            //now we turn until our encoders reach a certain value 
            motor(0, -70);
            motor(2, 70);
            reset_encoder(right_enc);
            reset_encoder(left_enc);
            while(((enc0=read_encoder(right_enc))<ticks_2_turn)&&((enc1=read_encoder(left_enc))<ticks_2_turn))
              {
                if(enc0>ticks_2_turn)
                  motor(right_motor,0);
                if(enc1>ticks_2_turn)
                  motor(left_motor,0);
            }
            
            // back up so the sensors are where the robot's center was
            break_all_motors();
            motor(0, -60);
            motor(2, -60);
            sleep(0.5);
            break_all_motors();
            
            // allign with the front stripe of the box
            sleep(0.25);
            getSet();
            sleep(0.2);
            
            // go again
            rampup();
            reset_encoder(right_enc);
            reset_encoder(left_enc);
            resets = 0;
        }
        
        // So we don't have a problem with rollover, break the 
        // 6ft distance into chunks and periosically reset the encoders
        if((enc0>ticks_2_dist)||(enc1>ticks_2_dist))
          {
            reset_encoder(right_enc);
            reset_encoder(left_enc);
            resets++;
        }
        
        corr_err();
        
        // if we've made three laps, we're done
        if (count==12)
          {
            alloff();
            return();
        }
    }
    alloff();
} 

/////////////////////////////////////////////////////////////////
// void rampup()
//    slowly raise the motors to desired power to reduce jerking
/////////////////////////////////////////////////////////////////
void rampup()
{
    int j;
    for(j=1; j<=8; j++)
      {
        sleep(0.1);
        motor(right_motor, (int)(rightspeed*j)/8);
        motor(left_motor, (int)(leftspeed*j)/8);
    }
}

/////////////////////////////////////////////////////////////////
// void corr_err()
//    determine if motor speeds need to be adjusted
//    based on encoder counts and then adjust them
/////////////////////////////////////////////////////////////////
void corr_err()
{
    
    int diff = enc1-enc0;// positive is right drift
    int half_diff=(int)(diff/2);
    // adjust motor speeds slightly if encoder counts are off
    if(diff > 0)
      {
        printf("right drift");
        rightspeed+=half_diff;
        leftspeed-=half_diff;
        motor(right_motor, rightspeed);
        motor(left_motor, leftspeed);
    }
    if(diff < 0)
      {
        printf("left drift");
        leftspeed-=half_diff;
        rightspeed+=half_diff;
        motor(right_motor, rightspeed);
        motor(left_motor, leftspeed);
    }
}

/////////////////////////////////////////////////////////////////
// void break_all_motors()
//   oscilates commands to the motors, causing them to 
//   lock up and halts movement quickly
/////////////////////////////////////////////////////////////////
void break_all_motors()
{
    int j;
    for(j=1;j<30;j++)
      {
        motor(0,-50);
        motor(2,-50);
        motor(0,50);
        motor(2,50);
    }
    ao();
}

/////////////////////////////////////////////////////////////////
// void getSet()
//   uses the refectance sesors to allign the robot
//   with the next black line it sees.
/////////////////////////////////////////////////////////////////
void getSet()
{
    int ana0, ana1;
    while(1)
      {
        ana0=analog(2);
        ana1=analog(3);
        // if both sensors soo black, call that o.k.
        if((ana0>thres)&&(ana1>thres))
          break;
        
        // now, for each motor, if it's corresponding
        //  sensor sees black, go backward.  if not,
        //  go forward.  The oscilation causes it to
        //  rapidly line up correctly.
        if(ana0>thres)
          motor(0,-40);
        else
          motor(0,40);
        
        if(ana1>thres)
          motor(2,-40);
        else
          motor(2,40);
        
    }
    break_all_motors();
}