Multiple Intersection Chooser Code
//
import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
import josx.platform.rcx.TextLCD;
import josx.platform.rcx.LCD;
import josx.platform.rcx.Sound;
/**
* MultipleIntersectionChooser.java
* Given: multiple intersections to choose from and an ordering of spoke numbers to take at successive intersections.
* The robot when determining it is at an intersection, will turn left a bit, then rotate clockwise starting (inclusive)at 12:00
* counting spokes. When it hits the deired spoke, it resumes intersection finder behavior.
* Once done with iterating through array of spokes, the program terminates.
* Can optionally change code to reset index to 0 if you hit the end of the array to allow continuous looping.
*
*
* @author CS371 2002
* @version 1.0
* @since 1.0
*/
public class MultipleIntersectionChooser extends JavaRCX {
public MultipleIntersectionChooser (){
}
/**
* The order of spokes to take as robot comes to different intersections.
* This order is generated uing a search of the map to find the shortest
* path to from the start to the goal.
*/
public static int[] SPOKES;
/**
* The index of SPOKES that keeps track of which spoke is next
*/
public static int index = 0;
/**
* Runs forever, taking appropriate motor actions for each state it is in.
* The while loops within some statements attempt to fine tune the robot
* so it is moved back onto the line in the CENTER_ONLY state.
* @param args a String[] value, command line arguments (none in our case)
*/
public static void main(String[] args){
int[][] map = new int[11][4];
map[0][0] = 1; map[0][1] = -1; map[0][2] = 8; map[0][3] = 4;
map[1][0] = 2; map[1][1] = -1; map[1][2] = 0; map[1][3] = 5;
map[2][0] = 3; map[2][1] = -1; map[2][2] = 1; map[2][3] = 5;
map[3][0] = 7; map[3][1] = -1; map[3][2] = 2; map[3][3] = 6;
map[4][0] = 5; map[4][1] = 0; map[4][2] = -1; map[4][3] = 8;
map[5][0] = 2; map[5][1] = 1; map[5][2] = 4; map[5][3] = 9;
map[6][0] = 7; map[6][1] = 3; map[6][2] = -1; map[6][3] = 10;
map[7][0] = -1; map[7][1] = 3; map[7][2] = 6; map[7][3] = 10;
map[8][0] = 9; map[8][1] = 4; map[8][2] = 0; map[8][3] = -1;
map[9][0] = 10; map[9][1] = 5; map[9][2] = 8; map[9][3] = -1;
map[10][0] = 7; map[10][1] = 6; map[10][2] = 9; map[10][3] = -1;
/** an alternate map
int[][] map = new int[9][4];
map[0][0] = 1; map[0][1] = -1; map[0][2] = 4; map[0][3] = 2;
map[1][0] = 7; map[1][1] = -1; map[1][2] = 0; map[1][3] = 7;
map[2][0] = 3; map[2][1] = 0; map[2][2] = -1; map[2][3] = 8;
map[3][0] = 4; map[3][1] = -1; map[3][2] = 2; map[3][3] = 5;
map[4][0] = -1; map[4][1] = 0; map[4][2] = 3; map[4][3] = 6;
map[5][0] = 6; map[5][1] = 3; map[5][2] = -1; map[5][3] = 8;
map[6][0] = 7; map[6][1] = 4; map[6][2] = 5; map[6][3] = -1;
map[7][0] = 1; map[7][1] = 1; map[7][2] = 6; map[7][3] = 8;
map[8][0] = 7; map[8][1] = 5; map[8][2] = 2; map[8][3] = -1;
**/
//direction the robot is coming from
int comeFrom = 0;
//starting intersection
int start = 4;
//ending intersection
int goal = 10;
SearchTest st = new SearchTest(map, start, goal);
int[] path = st.findit();
Translator trans = new Translator(map);
int[] goalPath = trans.translate(comeFrom, path);
SPOKES = goalPath;
initialize();
while (true) {
if(state == NONE)
lost();
/*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 is 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 sensor (s2) is on black, then sleep for approx < 1/8 of a
second. This 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)
*/
else if(state == LEFT_ONLY){
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, so no call to isIntersection() will occur here.
* 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 of the sensors that called this method.
* @return a boolean value, whether or not the robot is at an intersection.
*/
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 index of SPOKES above and that the robot is at an intersection
* it will rotate to SPOKES[index] by counting black lines it crosses.
* This method is thread safe from main, so calls to isIntersection() will not be found here.
*
*/
public static void chooseSpoke(){
/*-beep to indicate which spoke the robot will turn at
-# of beeps indicates number of spokes to turn
-spokes are chosen turning clockwise (right)
-first spoke is north (straight ahead), if there is a spoke at that position
*/
for (int i = 0; i < SPOKES[index]; i++){
Sound.beep();
try{Thread.sleep(300);}
catch(InterruptedException e){}
}
//stop robot before making slight left turn
try{
Thread.sleep(1000);
}
catch(InterruptedException e){}
/*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
won't get it on white to begin with. When this happens, it
generally starts on the spoke to the left, which cause the
robot to end one spoke too early. This didn't happen often,
but it is something that needs to be worke out. So be sure
the turn gets it to the left of the line north of its front
(if there is a line in that position).
*/
Motor.A.backward();
Motor.C.backward();
int spokes = 0;
//assume robot is to left of north (straight ahead) line on white
while(spokes < SPOKES[index]){
/*This counts the spokes, starting (if there is one) with the spoke directly
in front of the robot (the north spoke) and rotating to the right (clockwise).
By having it look for black first, we increase the chance that it will count
the first line facing north , since it is starting on white due to the left
turn that was done above (at the beginning of chooseSpoke()). If the spoke
count has not been reached, the robot then rotates right until it reaches white.
This sets it up for the next turn. The robot stops when it has reached the
target spoke.
*/
//rotate until hits black
while(Sensor.S2.readValue() > DARK){
Motor.A.forward();
Motor.C.forward();
}
//robot has hit spoke, increment spoke count
spokes++;
//if we have not hit the final spoke, move the robot to white to set up for the next turn
if (spokes < SPOKES[index])
//rotate until hits white
while(Sensor.S2.readValue() <= DARK){
Motor.A.forward();
Motor.C.forward();
}
/*have robot pause 1.5 seconds so the light sensors are not rushed in
reading. We found if you scan non-stop, you get weird spoke counts.
*/
try{
Motor.A.stop();
Motor.C.stop();
Thread.sleep(1500);
}
catch(InterruptedException e){
}
}
//increment index of spoke array to prepare for the next intersection
index++;
//if we have gone through every spoke in SPOKES, terminate
if(index == SPOKES.length){
TextLCD.print("YIPEE");
Sound.systemSound(false,2);
Sound.systemSound(false,3);
powerOff();
}
//If we are not done, have robot continue looking for intersections
Motor.A.forward();
Motor.C.backward();
}
}// MultipleIntersectionChooser