What version of NetBeans? 6.9.1
What operating system? Win vista
What version of Java? jdk1.6.0_24
- Code: Select all
import lejos.nxt.*;
import lejos.nxt.LCD;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.addon.IRSeeker;
import lejos.robotics.navigation.*;
/**
*
* Requires a wheeled vehicle with three independently controlled
* motors connected to motor ports A, B and C, and
* a compass sensor connected to sensor port 1,
* a infrared seeker sensor connected to sensor port 2 and
* an ultrasonic sensor connected to sensor port 3;
*
* @author XXX
*
*/
public class MainPro {
private CompassSensor compass = new CompassSensor(SensorPort.S1); //create an instance of CompassSensor
private IRSeeker seeker = new IRSeeker(SensorPort.S2); //create an instance of IRSeeker
private UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S3); //create an instance of UltrasonicSnsor
private CompassPilot pilot = new CompassPilot(compass, 5.6f, 14.1f, Motor.C, Motor.A); //create an instance of Pilot
private SimpleNavigator vehicle = new SimpleNavigator(pilot);
int pLong = 68, pWidth = 52;
private void turn(final int left, int right) {
new Thread() {
public void run() {
Motor.C.rotate(left);
//vehicle.rotate(dergress);
}
}.start();
Motor.A.rotate(right);
}
/*private void direction(final int dir1) {
int[] dirArray={0,-120,-90,-60,-30,0,30,60,90,120};
vehicle.rotate(dirArray[dir1]);
vehicle.stop();
dir=seeker.getDirection();
heading = (int)compass.getDegrees();
if (dir==5 && heading==0) {
vehicle.goTo(20, 0);
return;
}
if (dir==5 && heading!=0) {
if (heading<180 || heading==180)
vehicle.rotate(-heading);
else if (heading>180)
vehicle.rotate(360-heading);
vehicle.stop();
dir=seeker.getDirection();
vehicle.updatePosition();
vehicle.goTo(20, 0);
return;
}
}*/
public void seek() {
int dir=seeker.getDirection();
int dis=sonic.getDistance();
LCD.drawInt(dir, 1, 0);
LCD.drawInt(dis, 1, 3);
do {
if (dir==0) {
dir=seeker.getDirection();
LCD.drawInt(dir, 1, 0);
}
if (dir==5) {
turn(50,50);
dir=seeker.getDirection();
LCD.drawInt(dir, 1, 0);
vehicle.stop();
LCD.drawInt(dis, 1, 3);
if(dis>112 && dis<115){
Motor.B.rotate(-80);
Motor.B.stop();
//break;
}
Motor.B.rotate(-80);
Motor.B.stop();
break;
//vehicle.rotateTo(0);
} else if (dir>5) {
turn(50,-50);
dir=seeker.getDirection();
LCD.drawInt(dir, 1, 0);
if (dir==5) {
turn(50,50);
dir=seeker.getDirection();
LCD.drawInt(dir, 1, 0);
vehicle.stop();
LCD.drawInt(dis, 1, 3);
if(dis>112 && dis<115){
Motor.B.rotate(-80);
Motor.B.stop();
//break;
}
Motor.B.rotate(-80);
Motor.B.stop();
break;
//vehicle.rotateTo(0);
}
} else if (dir>0 && dir<5) {
turn(-50,50);
dir=seeker.getDirection();
LCD.drawInt(dir, 1, 0);
if (dir==5) {
turn(50,50);
dir=seeker.getDirection();
LCD.drawInt(dir, 1, 0);
vehicle.stop();
LCD.drawInt(dis, 1, 3);
if(dis>112 && dis<115){
Motor.B.rotate(-80);
Motor.B.stop();
//break;
}
//vehicle.rotateTo(0);
Motor.B.rotate(-80);
Motor.B.stop();
break;
}
}
} while (dir!=5);
int heading=(int)compass.getDegrees();
LCD.drawInt(heading, 1, 1);
do {
if (heading==0) {
vehicle.stop();
vehicle.goTo(11, 20);
Motor.B.setSpeed(720);
Motor.B.setPower(100);
Motor.B.rotate(75);
Motor.B.stop();
Motor.B.rotate(-45);
Motor.B.stop();
LCD.drawInt((int)vehicle.getX(), 1, 4);
LCD.drawInt((int)vehicle.getY(), 1, 5);
return;
} else if(heading<180) {
//vehicle.rotate(-heading);
vehicle.rotate(360, true);
while(pilot.isRotating())
{
heading=(int)compass.getDegrees();
if(heading==0)
{
vehicle.stop();
vehicle.goTo(-3, 2);
Motor.B.setSpeed(720);
Motor.B.setPower(100);
Motor.B.rotate(75);
Motor.B.stop();
Motor.B.rotate(-45);
Motor.B.stop();
LCD.drawInt((int)vehicle.getX(), 1, 4);
LCD.drawInt((int)vehicle.getY(), 1, 5);
return;
//break;
}
}
} else {
vehicle.rotate(360, true);
while(pilot.isRotating())
{
heading=(int)compass.getDegrees();
if(heading==0)
{
vehicle.stop();
vehicle.goTo(11, 20);
Motor.B.setSpeed(720);
Motor.B.setPower(100);
Motor.B.rotate(75);
Motor.B.stop();
Motor.B.rotate(-45);
Motor.B.stop();
LCD.drawInt((int)vehicle.getX(), 1, 4);
LCD.drawInt((int)vehicle.getY(), 1, 5);
return;
//break;
}
}
}
} while (heading!=0);
/*if(dir==5 && heading==0) {
Motor.B.setSpeed(720);
Motor.B.setPower(100);
Motor.B.rotate(75);
Motor.B.stop();
Motor.B.rotate(-45);
Motor.B.stop();
LCD.drawInt((int)vehicle.getX(), 1, 3);
LCD.drawInt((int)vehicle.getY(), 1, 4);
return;
}*/
/*
while (heading>=0 && heading<360) {
heading = (int)compass.getDegrees();
if (heading==0) {
vehicle.stop();
}
if (heading>0 && heading<360) {
vehicle.rotateTo(0);
}
}
dir=seeker.getDirection();
heading = (int)compass.getDegrees();
do {
dir=seeker.getDirection();
heading = (int)compass.getDegrees();
if (dir==0) {
dir=seeker.getDirection();
}
if (dir==4) {
direction(4);
return;
} else if (dir==3) {
direction(3);
return;
} else if (dir==2) {
direction(2);
return;
} else if (dir==1) {
direction(1);
return;
} else if (dir==6) {
direction(6);
return;
} else if (dir==7) {
direction(7);
return;
} else if (dir==8) {
direction(8);
return;
} else if (dir==9) {
direction(9);
return;
}
if (dir==5 && heading==0) {
dir=seeker.getDirection();
vehicle.goTo(20, 0);
return;
}
if (dir==5 && heading!=0) {
dir=seeker.getDirection();
if (heading<180 || heading==180)
vehicle.rotateTo(0);
else if (heading>180)
vehicle.rotateTo(0);
vehicle.stop();
dir=seeker.getDirection();
vehicle.goTo(20, 0);
return;
}
} while(dir!=5 || heading!=0);*/
}
public MainPro() {
seeker.setAddress(0x8);
sonic.continuous();
vehicle.setMoveSpeed(100);
vehicle.setTurnSpeed(50);
vehicle.goTo(0,20);
//vehicle.goTo(pLong/2, pWidth/2);
//vehicle.stop();
Motor.A.setSpeed(100);
Motor.C.setSpeed(100);
/*while(true) {
seek();
}
*/
//vehicle.goTo(0, 10);
/*while (heading!=0) {
heading();
}*/
}
public static void main(String st[]) {
Button.ESCAPE.addButtonListener(new ButtonListener() //add a button listener to ESCAPE button
{
public void buttonPressed(Button b) {System.exit(0);} //program finish if pressed ESCAPE button
public void buttonReleased(Button b) {}
});
MainPro p = new MainPro();
}
}
I got a problem. I used goTo to make robot go to north after catched ir ball but it was not go to north every time. Anyone know how to fix?
I want to rewrite the code using behaviour programming. I tried to rewrite it but not success. Anyone give me some hints to rewrite it. Thx!
