IntersectionFinder Code

//

import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;




/**
 * IntersectionFinder.java
 * Capable of finding a T from each path, A four way perpendicular intersection, and forms of Y's.
 * Restriction:  Angles must not be smaller than 90 degrees.
 * @author CS371 2002
 * @version 1.0
 * @since 1.0
 */
public class IntersectionFinder extends JavaRCX{
    public IntersectionFinder (){
    }
  
    /**
     * Whether or not an intersection has been found.
     */
    public static boolean found = false;
        
    /**
     * Runs forever taking appropriate motor actions for each state it is in.
     * While loops within some statements attempt to fine tune the robot so it is back on the line in the CENTER_ONLY state.
     * Program Execution will terminate if an intersection is found.
     * @param args a String[] value, command line arguments, none needed here.
     */
    public static void main(String[] args){
	initialize();
	while (!found) {
	    //if(searching4X){
		if(state == NONE)
		    lost();
		else if(state == LEFT_ONLY){
		    //adjust left until center in on black
		    while(Sensor.S2.readValue() > DARK){
			Motor.C.backward();
			Motor.A.backward();
		    }
		}
		else if(state == CENTER_ONLY){
		    //move forward
		    Motor.A.forward();
		    Motor.C.backward();
		}
		else if(state == RIGHT_ONLY){
		    //adjust right until center is on black
		    while(Sensor.S2.readValue() > DARK){
			Motor.A.forward();
			Motor.C.forward();
		    }
		}
		else if(state == LEFT_CENTER){
		    if(isIntersection(LEFT_CENTER))
			powerOff();
		    //adjust until center is on black
		    while(Sensor.S2.readValue() > DARK){
			Motor.A.backward();
			Motor.C.backward();
		    }
		}
		else if(state == LEFT_RIGHT){
		    if(isIntersection(LEFT_RIGHT)){
			powerOff();
		    }
		}
		else if(state == CENTER_RIGHT){
		    if(isIntersection(CENTER_RIGHT))
			powerOff();
		    //adjust until center is on black
		    while(Sensor.S2.readValue() > DARK){
			Motor.A.forward();
			Motor.C.forward();
		    }
		}
		else if(state == ALL){
		    if(isIntersection(ALL))
			powerOff();
		}
	}
    }

  
  /**
   * Determines whether at an intersection or not.  This method is thread safe because it is called from within main.
   * The watcher thread has no effect on anything here, it only goes back into effect when execution returns to the main.
   * @param triggerState an int value, the state that caused the call to this method.
   * @return a boolean value, whether or not an intersection has been found.
   */
  public static boolean isIntersection(int triggerState){
    /*Move robot forward for 1/8 second which will allow you to judge whether it is an intersection
      based on the triggering state which called isIntersection and the state the robot is in after the short roll forward.
    */
    Motor.A.forward();
    Motor.C.backward();
    try{
      Thread.sleep(125);
    }
    catch(InterruptedException e){}
    finally{
      Motor.A.stop();
      Motor.C.stop();
    }
    
    /*Read values after the roll foward*/
    if(Sensor.S1.readValue() > DARK && Sensor.S2.readValue() > DARK && Sensor.S3.readValue() > DARK){
      /*Check to make sure the CENTER_RIGHT is not a corner, but an intersection such as a T or Y.
	Therefore it does a scan left, then returns to its original location.
	We had chose to sleep the Thread 1/4 of a second.
      */
      if(triggerState == CENTER_RIGHT){
	Motor.A.backward();
	Motor.C.backward();
	try{
	  Thread.sleep(250);
	}
	catch(InterruptedException e){
	}
	Motor.A.stop();
	Motor.C.stop();
	
	/*
	  If when the thread wakes up and a sensor is on a black line, we know we hit an intersection of some kind.
	  Possible problem here with given sensor configuration although we have never seen it happen is that the 
	  line could be resting in between two sensors. Highly unlikely but something for next group to think about.
	  Maybe Scan possibly one more short distance to the left which would move sensors to where the gaps between them were previously.
	  This would be a very short sleep (<100millis)
	*/
	if((Sensor.S1.readValue() <= DARK) || (Sensor.S2.readValue() <= DARK) || (Sensor.S3.readValue() <= DARK)){
	  return true;
	}
	else{
	  //it is a corner
	  Motor.A.forward();
	  Motor.C.forward();
	  try{
	    Thread.sleep(250);
	  }
	  catch(InterruptedException e){
	  }
	  Motor.A.stop();
	  Motor.C.stop();
	  return false;
	}
      }
      /*Check to make sure the LEFT_CENTER is not a corner, but an intersection such as a T or Y.
	Therefore it does a scan right, then returns to its original location.
	We had chose to sleep the thread 1/4 of a second
      */
      if(triggerState == LEFT_CENTER){
	Motor.A.forward();
	Motor.C.forward();
	try{
	  Thread.sleep(250);
	}
	catch(InterruptedException e){
	}
	Motor.A.stop();
	Motor.C.stop();
	/*
	  If when the thread wakes up and a sensor is on a black line, we know we hit an intersection of some kind.
	  Possible problem here with given sensor configuration although we have never seen it happen is that the 
	  line could be resting in between two sensors. Highly unlikely but something for next group to think about.
	  Maybe Scan possibly one more short distance to the right which would move sensors to where the gaps between them were previously.
	  This would be a very short sleep (<100millis)
	*/
	if((Sensor.S1.readValue() <= DARK) || (Sensor.S2.readValue() <= DARK) || (Sensor.S3.readValue()<= DARK)){
	  return true;
	}
	else{
	  //it is a corner
	  Motor.A.backward();
	  Motor.C.backward();
	  try{
	    Thread.sleep(250);
	  }
	  catch(InterruptedException e){
	  }
	  Motor.A.stop();
	  Motor.C.stop();
	  return false;
	}
      }
    }
    //must be an intersection
    return true;
  }
    
}// IntersectionFinder

//