line.ic

/**
 * @file line.ic
 * @brief Code to make the robot drive along the metal lines.
 * @author Andrew Krieger and Joel Friedly
 * @date 5/23/2011
 */

#use "defines.ic"
#use "util.ic"
#use "drive.ic"
#use "servo/exp_servo_lib.ic"
#use "servo/exp_servo_calibrate.ic"
#use "CdS.ic"
#use "line.ic"
#use "turn-slow.ic"
#use "turn.ic"
#use "step.ic"
#use "gps.ic"
#use "line.ic"


/// Use bitmasks to make it easier to read the sensors
#define BACK_LEFT_BIT   0
#define BACK_RIGHT_BIT  1
#define FRONT_LEFT_BIT  2
#define FRONT_RIGHT_BIT 3

#define BACK_LEFT_MASK   (1<<BACK_LEFT_BIT)
#define BACK_RIGHT_MASK  (1<<BACK_RIGHT_BIT)
#define FRONT_LEFT_MASK  (1<<FRONT_LEFT_BIT)
#define FRONT_RIGHT_MASK (1<<FRONT_RIGHT_BIT)


/// Check both front optosensors
int opto_mask_front() {
    return ((analog(OPTO_FRONT_LEFT)  < OPTO_THRESHOLD) <<  FRONT_LEFT_BIT)
      |    ((analog(OPTO_FRONT_RIGHT) < OPTO_THRESHOLD) << FRONT_RIGHT_BIT);
}

/// Check both rear optosensors
int opto_mask_back() {
    return ((analog(OPTO_BACK_LEFT)   < OPTO_THRESHOLD) <<   BACK_LEFT_BIT)
      |    ((analog(OPTO_BACK_RIGHT)  < OPTO_THRESHOLD) <<  BACK_RIGHT_BIT);
}

/// Check all optosensors
int opto_mask_all() {
    return opto_mask_front() | opto_mask_back();
}

/**
 * @brief Line up with metal strip
 *
 * This is meant to be used to align with the corn.
 * Turn CW and drive straight to align with metal strip.
 */
void align_forward_cw(void) {
    int mask;
    printf("Align forward cw\n");
    
    for(;;) {
        mask = opto_mask_all();
        
        if(mask & BACK_RIGHT_MASK) {
            ao();
            return;
        }
        
        if(mask & FRONT_LEFT_MASK) {
            motor(MOTOR_LEFT, 100);
            motor(MOTOR_RIGHT, -100);
        } else {
            motor(MOTOR_LEFT, 100);
            motor(MOTOR_RIGHT, 100);
        }
    }
}


/**
 * @brief Line up with the metal strip
 *
 * This is meant to be used to align with the wagon.  Put the front optos just off of the metal strip,
 * so that if the robot drove forward, it would be over the strip.  Call this function, and it should
 * align with the strip.
 * Problems: (1) You have to be ~4-5 inches from the end of the strip, or the robot will drive off
 * and get lost.  (2) Doesn't always end with both pairs of sensors straddling the boundary of the strip
 */
void align_back_cw(void) {
    int mask, last;
    printf("Align back cw\n");
    
    for(;;) {
        mask = opto_mask_all();
        if(mask & BACK_RIGHT_MASK) {
            ao();
            return;
        }
        
        if((mask & (FRONT_LEFT_MASK | FRONT_RIGHT_MASK)) == FRONT_LEFT_MASK) {
            motor(MOTOR_LEFT, 100);
            motor(MOTOR_RIGHT, 100);
        } else { //No optos on strip - assume we went off the end and are about lined up
            motor(MOTOR_LEFT, 100);
            motor(MOTOR_RIGHT, -100);
        }
        
    }
}


/**
 * @brief Follow a metal strip forwards by reading the front optosensors.
 * @param left Tells it whether to follow the left or right boundary (Boolean)
 * @param timeout Timeout period in ms
 *
 * Works by running a motor at 100% power until the left optosensor crosses the boundary, then it runs the other motor at 100% power.  
 * Finds the end of the strip by waiting until the right optosensor loses the strip.
 * Times out after 7.5s.
 */
void follow_strip_forward(int left, long timeout) {
    int i;
    int edge_port = OPTO_FRONT_RIGHT, end_port = OPTO_FRONT_LEFT;
    int on_motor = MOTOR_LEFT, off_motor = MOTOR_RIGHT;
    int last;
    
    if(left) {
        edge_port = OPTO_FRONT_LEFT;
        end_port = OPTO_FRONT_RIGHT;
        on_motor = MOTOR_RIGHT;
        off_motor = MOTOR_LEFT;
    }
    
    reset_system_time();
    
    for(i=0; i<3; i++) {
        for(;;) {
            while(analog(end_port) < OPTO_THRESHOLD && mseconds() < timeout) {
                if(analog(edge_port) < OPTO_THRESHOLD) {
                    motor(on_motor, 100);
                    motor(off_motor, 0);
                    last = 1;
                } else {
                    motor(on_motor, 0);
                    motor(off_motor, 100);
                    last = 0;
                }
            }
            
            printf("End:%d Edge:%d\n", analog(end_port), analog(edge_port));
            
            msleep(100L);
            //If the last was on, and on == RIGHT, or if last was off, and off == RIGHT, then step right
            if(last ^ (on_motor == MOTOR_LEFT))
              step_treads(1, 0);
            //Otherwise step left
            else
              step_treads(0, 1);
            if(analog(end_port) >= OPTO_THRESHOLD || mseconds() >= timeout) {
                printf("End:%d Edge:%d\n", analog(end_port), analog(edge_port)); 
                msleep(250L);
                break;
            }
        }
    }
}


/**
 * @brief Follow the strip backward.
 * @param left Boolean to follow the left or right boundary
 * @param timeout Timeout period in ms
 *
 * Note:  The function had some bugs in it that were never worked out.
 * It was intended to replace @ref follow_strip_back, but never did because it 
 * didn't work consistently.
 */
void follow_strip_backward(int left, long timeout) {
    int i;
    int edge_port = OPTO_FRONT_RIGHT, end_port = OPTO_FRONT_LEFT;
    int on_motor = MOTOR_LEFT, off_motor = MOTOR_RIGHT;
    
    if(left) {
        edge_port = OPTO_FRONT_LEFT;
        end_port = OPTO_FRONT_RIGHT;
        on_motor = MOTOR_RIGHT;
        off_motor = MOTOR_LEFT;
    }
    
    reset_system_time();
    
    for(i=0; i<3; i++) {
        for(;;) {
            while(analog(end_port) < OPTO_THRESHOLD && mseconds() < timeout) {
                if(analog(edge_port) < OPTO_THRESHOLD) {
                    motor(on_motor, -100);
                    motor(off_motor, 0);
                } else {
                    motor(on_motor, 0);
                    motor(off_motor, -100);
                }
            }
            
            ao();
            printf("End:%d Edge:%d\n", analog(end_port), analog(edge_port));
            
            msleep(100L);
            motor(off_motor, -100);
            motor(on_motor, 0);
            msleep(100L);
            ao();
            if(analog(end_port) >= OPTO_THRESHOLD || mseconds() >= timeout) {
                printf("End:%d Edge:%d\n", analog(end_port), analog(edge_port)); 
                break;
            }
        }
    }
}


/**
 * @brief Follow a metal strip backwards by reading the back optosensors.
 * @param left Tells it whether to follow the left or right boundary (1 or 0)
 *
 * Works by running a motor at 100% power until the left optosensor crosses the boundary, then it runs the other motor at 100% power.  
 * Finds the end of the strip by waiting until the right optosensor loses the strip.
 * Times out after 7.5s.
 */
void follow_strip_back(int left) 
{    
    int readingL, readingR;
	
    //follow back along left boundary
    if(left) 
      {
        reset_system_time();
        printf("L%d R%d\n", analog(OPTO_BACK_LEFT), analog(OPTO_BACK_RIGHT));
        for(;;)
          {
            while(analog(OPTO_BACK_RIGHT) < OPTO_THRESHOLD && mseconds() < 7500L)
              {
                readingL=analog(OPTO_BACK_LEFT);
                if(readingL > OPTO_THRESHOLD)
                  {
                    motor(MOTOR_LEFT, -100);
                    motor(MOTOR_RIGHT, 0);
                    readingL=analog(OPTO_BACK_LEFT);
                }
                
                else //if(readingL < OPTO_THRESHOLD)
                  {
                    motor(MOTOR_LEFT, 0);
                    motor(MOTOR_RIGHT, -100);
                    readingL=analog(OPTO_BACK_LEFT);
                }
            }
            
            msleep(5L);
            ao();
            if(analog(OPTO_BACK_RIGHT) >= OPTO_THRESHOLD) {
                printf("%d %d\n", analog(OPTO_BACK_LEFT), analog(OPTO_BACK_RIGHT));
                break;
            }
        }
    }

	else {
    	reset_system_time();
    
        while(analog(OPTO_BACK_LEFT) < OPTO_THRESHOLD && mseconds() < 7500L) {
            readingR=analog(OPTO_BACK_RIGHT);
            if(readingR > OPTO_THRESHOLD) {
                    motor(MOTOR_RIGHT, -100);
                    motor(MOTOR_LEFT, 0);
                    readingR=analog(OPTO_BACK_RIGHT);
        	}
                
            else {
                    motor(MOTOR_RIGHT, 0);
                    motor(MOTOR_LEFT, -100);
                    readingR=analog(OPTO_BACK_RIGHT);
        	}
        msleep(5L);
        ao();
   	 	}
	}
}