Intersection Chooser Code
//
import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
/**
* IntersectionChooser.java
* Given a spoke number, the robot when determining it is at an intersection
* will turn left, rotate clockwise starting(inclusive)at 12:00 counting spokes and when it
* hits the nth spoke resume execution of intersection finder.
* @author CS371 2002
* @version 1.0
* @since 1.0
*/
public class IntersectionChooser extends JavaRCX{
public IntersectionChooser (){
}
/**
* Which spoke to take when getting to an intersection.
* Change only this variable to take different spokes.
*/
public static final int SPOKE = 3;
/**
* 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.
* @param args a String[] value, command line arguements, none in this case.
*/
public static void main(String[] args){
initialize();
while (true) {
if(state == NONE)
lost();
else if(state == LEFT_ONLY){
/*We found that without sleeping for a bit here the robot will violently go back and forth between two sensors.
It will get where it needs to go but its very unelegant.
So here we are programming smoothness into the travel path.
If way off such as Left sensor only. Have it move until the center is on black, then sleep for approx < 1/8 of a second
which will cause the robot to continue coasting in its current direction. When it wakes up the state should be
close to the center sensor being exactly in the center of the black line rather than on the side.
Note: You can tweak this value as necessary,(<100ms) be sure to update the RIGHT_ONLY as well.
The same goes for LEFT_CENTER and CENTER_RIGHT, although this state is closer to the actual center of the line
so sleep time should be a lot less. (<30ms)
*/
while(Sensor.S2.readValue() > DARK){
Motor.C.backward();
Motor.A.backward();
}
try{
Thread.sleep(100);
}
catch(InterruptedException e){
}
}
else if(state == CENTER_ONLY){
Motor.A.forward();
Motor.C.backward();
}
else if(state == RIGHT_ONLY){
while(Sensor.S2.readValue() > DARK){
Motor.A.forward();
Motor.C.forward();
}
try{
Thread.sleep(100);
}
catch(InterruptedException e){
}
}
else if(state == LEFT_CENTER){
if(isIntersection(LEFT_CENTER))
chooseSpoke();
while(Sensor.S2.readValue() > DARK){
Motor.A.backward();
Motor.C.backward();
}
try{
Thread.sleep(30);
}
catch(InterruptedException e){
}
}
else if(state == LEFT_RIGHT){
if(isIntersection(LEFT_RIGHT)){
chooseSpoke();
}
}
else if(state == CENTER_RIGHT){
if(isIntersection(CENTER_RIGHT))
chooseSpoke();
while(Sensor.S2.readValue() > DARK){
Motor.A.forward();
Motor.C.forward();
}
try{
Thread.sleep(30);
}
catch(InterruptedException e){
}
}
else if(state == ALL){
if(isIntersection(ALL))
chooseSpoke();
}
}
}
/**
* 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
* @return a boolean value
*/
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 was 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 was a corner
Motor.A.backward();
Motor.C.backward();
try{
Thread.sleep(250);
}
catch(InterruptedException e){
}
Motor.A.stop();
Motor.C.stop();
return false;
}
}
}
//we know it is an itersection
return true;
}
/**
* Given the value of SPOKE above and that the robot is at an intersection
* it will rotate to SPOKE and it thread safe from main, so no new calls to isIntersection() will be fired here.
*/
public static void chooseSpoke(){
/*Move robot slightly to left 1/8 sec
You may need to tweak this try between 1/8 and 1/4 second,
Sometimes robot comes in at weird angle and the turn left wont get it on white to begin with.
It would still be on the north black line and wind up going an extra spoke. So be sure the twitch gets it to the
left of the line north of its front.
*/
Motor.A.backward();
Motor.C.backward();
try{
Thread.sleep(125);
}
catch(InterruptedException e){
}
int spokes = 0;
//assume robot is to left of north line on white
while(spokes < SPOKE){
//LCD.showNumber(spokes);//debug
/*by having it look for black first we increase the chance that it will count the first line facing
north because if it is white when it did the little twitch left then we move until hit black
otherwise it is black and we skip the loop.
Rotate until hit black
*/
while(Sensor.S2.readValue() > DARK){
Motor.A.forward();
Motor.C.forward();
}
//rotate until hit white
while(Sensor.S2.readValue() <= DARK){
Motor.A.forward();
Motor.C.forward();
}
//robot has now passed a line, increase spoke count
spokes++;
/*have robot pause 1.5 seconds this way we are positive light sensors are not rushed in reading.
*we found if you continously scan non-stop you get weird counts, better to be slower and accurate than fast and inaccurate.
*/
try{
Motor.A.stop();
Motor.C.stop();
Thread.sleep(1500);
}
catch(InterruptedException e){
}
}
//Have robot start moving forward onto new spoke
Motor.A.forward();
Motor.C.backward();
}
}// IntersectionChooser
//