I've done some part of project about maze.
The main idea was to create a robot, which can go through a maze. I need to send data via Bluetooth and draw the whole maze. Surface of the maze should be white. There should be 2 points (or more) with a different colour in the maze. The robot should find these points while dissevering the maze. After drawing whole maze robot should know the location of that points and go to point No.1. Then the robot should go to the pont No.2 with a shortest way.
The code:
- Code: Select all
public static void main(String [] args){
connect();
while(true) {
control = checkCommand();
if (control)
move();
}
}//End main
public static boolean checkCommand()//check input data {
boolean start = false;
try {
transmitReceived = dataIn.readInt();
if (transmitReceived == 1) start = true;
if (transmitReceived == 2) start = false;
}
catch (IOException ioe) { System.out.println("IO Exception readInt"); }
return start;
}//End checkCommand
public static void move() {
Pilot pilot = new TachoPilot(1.7f, 4.3f, Motor.A, Motor.C, true);
UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S3);
ColorLightSensor colour = new ColorLightSensor(SensorPort.S2, ColorLightSensor.TYPE_COLORFULL);
int d = 8, count = 0; // d - inches
int direction = 0; // 0 - north (0'), 1 - east (90'), 2 - south (180'), 3 - west(270')
boolean rightWall, leftWall, forwardWall;
int movingSpeed = 600;
Motor.A.setSpeed(movingSpeed);
Motor.C.setSpeed(movingSpeed);
Motor.A.setSpeed(400);
Motor.C.setSpeed(400);
float dist = sonic.getDistance();
Motor.B.setSpeed(600);
count = Motor.B.getTachoCount();
while (!Button.ENTER.isPressed()){
sonic.getDistance();
rightWall = false;
leftWall = false;
forwardWall = false;
count = Motor.B.getTachoCount();
int wall = 0;
//Colors.Color color = colour.readColor(); // this will be used later when I'll finish drawing
//LCD.drawString(color.name(), 7, 3);
Motor.B.rotateTo(-90);
if (sonic.getDistance() > 15 )
rightWall = false;
else {
rightWall = true;
wall += 100;
}
Motor.B.rotateTo(0);
if (sonic.getDistance() > 15 )
forwardWall = false;
else {
forwardWall = true;
wall += 10;
}
Motor.B.rotateTo(90);
if (sonic.getDistance() > 15 )
leftWall = false;
else {
leftWall = true;
wall += 1;
}
Motor.B.rotateTo(0);
try {
dataOut.writeInt(direction); // sends integer to PC, to know which position it faces currently
dataOut.writeInt(wall); // send integer about walls
dataOut.flush();
} catch (IOException e) {
System.out.println("\nIO Exception writeInt from brick!!!");
}
// after sending data it moves using alphabetic order
if (rightWall == false){
direction += 1;
turnRight();
pilot.travel(-d);
} else
if (forwardWall == false){
try{Thread.sleep(500);} catch (Exception e){}
pilot.travel(-d);
} else if (leftWall == false) {
direction -= 1;
turnLeft();
pilot.travel(-d);
try{Thread.sleep(500);} catch (Exception e){}
} else {
direction += 2;
turnRight();
turnRight();
}
switch (direction) {
case -1: direction = 3;
case 4: direction = 0;
case 5: direction = 1;
}
}
Motor.B.rotateTo(0);
}//End move
public static void connect() { }//End connect
public static void turnRight(){
Motor.C.forward();
Motor.A.backward();
try{Thread.sleep(740);} catch (Exception e){}
Motor.A.stop();
Motor.C.stop();
}
public static void turnLeft(){
Motor.A.forward();
Motor.C.backward();
try{Thread.sleep(740);} catch (Exception e){}
Motor.A.stop();
Motor.C.stop();
}
}//NXTtr Class
My questions are:
1) Is my code right or there is better way to program it?
2) I didn't use navigation (special library for LeJOS), should use it, will it be easier?
3) How do avoid wall collisions? because robot doesn't turns exactly 90' degree.
So far I've done GUI part, but it doesn't work properly. I'm still have a war with a Threads and Swing Time...
Thanks to everyone who can help...