The robot only turn around to find the ball. However, the robot can't find the ball when I put the ball in front of the robot.
- Code: Select all
import lejos.robotics.subsumption.*;
import lejos.robotics.*;
import lejos.nxt.*;
import lejos.nxt.addon.*;
/**
*
* Requires a wheeled vehicle with three independently controlled
* motors connected to motor ports A, B and C, and
* a coloulightsensor connected to sensor port 1,
* a touchmsensor connected to sensor port 2 and
* an ultrasonic sensor connected to port 3;
*
* @author Lee Chun Fai
*
*/
public class SoccerRobot
{
public static void main(String[] args)
{
Motor.A.setSpeed(400);
Motor.B.setSpeed(400);
Motor.C.setSpeed(400);
Behavior b1 = new DriveForward();
Behavior b2 = new DetectWall();
Behavior b3 = new DetectBall();
Behavior b4 = new TurnAround();
Behavior b5 = new ShutDown();
Behavior[] behaviorList =
{
b1, b2, b3, b4, b5
};
boolean returnWhenInactive = true;
Arbitrator arbitrator = new Arbitrator(behaviorList, returnWhenInactive);
LCD.drawString("Soccer Robot",0,1);
Button.waitForPress();
arbitrator.start();
}
}
class DriveForward implements Behavior
{
private boolean _suppressed = false;
public boolean takeControl()
{
return true; // this behavior always wants control.
}
public void suppress()
{
_suppressed = true;// standard practice for suppress methods
}
public void action()
{
_suppressed = false;
Motor.A.backward();
Motor.C.backward();
while (!_suppressed)
{
Thread.yield(); //don't exit till suppressed
}
Motor.A.stop(); // not strictly necessary, but good probramming practice
Motor.C.stop();
}
}
class DetectWall implements Behavior
{
private boolean _suppressed = false;
public DetectWall()
{
color = new ColorSensor(SensorPort.S1);
touch = new TouchSensor(SensorPort.S2);
sonar = new UltrasonicSensor(SensorPort.S3);
}
public boolean takeControl()
{
return touch.isPressed() || sonar.getDistance() < 25 || color.getColorNumber()!=3;
}
public void suppress()
{
_suppressed = true;
}
public void action()
{
_suppressed = false;
Motor.A.rotate(180, true);// start Motor.A rotating backward
Motor.C.rotate(360); // rotate C farther to make the turn
}
private ColorSensor color;
private TouchSensor touch;
private UltrasonicSensor sonar;
}
class DetectBall implements Behavior
{
private boolean _suppressed = false;
public DetectBall()
{
color = new ColorSensor(SensorPort.S1);
}
public boolean takeControl()
{
return color.getColorNumber()==3;
}
public void suppress()
{
_suppressed = true;
}
public void action()
{
_suppressed = false;
Motor.B.rotate(-30); //start Motor.B rotating backward
Motor.B.rotate(30); //rotate further to kick ball
}
private ColorSensor color;
}
class TurnAround implements Behavior
{
private boolean _suppressed = false;
public TurnAround()
{
color = new ColorSensor(SensorPort.S1);
touch = new TouchSensor(SensorPort.S2);
sonar = new UltrasonicSensor(SensorPort.S3);
}
public boolean takeControl()
{
return !touch.isPressed() || sonar.getDistance() == 255 || color.getColorNumber()!=3;
}
public void suppress()
{
_suppressed = true;
}
public void action()
{
_suppressed = false;
//Motor.A.rotate(180, true);// start Motor.A rotating backward
Motor.C.rotate(360); // rotate C farther to make the turn
}
private ColorSensor color;
private TouchSensor touch;
private UltrasonicSensor sonar;
}
class ShutDown implements Behavior {
public boolean takeControl() {
return Button.ESCAPE.isPressed();
}
public void suppress() {
System.exit(0);
}
public void action() {
System.exit(0);
}
}
compile-selected-files-in-src:
NewMain.clean:
Deleting directory C:\Users\user\Documents\NetBeansProjects\FYP\build
NewMain.compile:
Created dir: C:\Users\user\Documents\NetBeansProjects\FYP\build
C:\Users\user\Documents\NetBeansProjects\FYP\build.xml:24: warning: 'includeantruntime' was not set, defaulting to build.sysclasspath=last; set to false for repeatable buildsCompiling 1 source file to C:\Users\user\Documents\NetBeansProjects\FYP\build
BUILD SUCCESSFUL (total time: 2 seconds)