Commit edc05fbb authored by Frederik Jerløv-Troelsen's avatar Frederik Jerløv-Troelsen
Browse files

Peder

parent aef19d50
/*
* Arbiter pass the highest priority car command
* from the behaviors to the car driver.
*/
public class Arbiter extends Thread
{
private SharedCar [] car;
private CarDriver cd;
private int winner;
public Arbiter(SharedCar [] car, CarDriver cd)
{
this.car = car;
this.cd = cd;
}
public void run()
{
while ( true )
{
for (int i=0; i < car.length; i++)
{
CarCommand carCommand = car[i].getCommand();
if ( carCommand != null)
{
cd.perform(carCommand);
winner = i;
break;
}
}
}
}
public int winner()
{
return winner;
}
}
import lejos.nxt.*;
import lejos.util.Delay;
/*
* Avoid behavior
*/
class Avoid extends Thread
{
private SharedCar car = new SharedCar();
private int power = 70, ms = 500;
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S2);
int frontDistance, leftDistance, rightDistance;
int stopThreshold = 30;
public Avoid(SharedCar car)
{
this.car = car;
}
public void run()
{
while (true)
{
// Monitor the distance in front of the car and stop
// when an object gets to close
frontDistance = sonar.getDistance();
while ( frontDistance >= stopThreshold )
{
car.noCommand();
frontDistance = sonar.getDistance();
}
car.stop();
Delay.msDelay(ms);
// Get the distance to the left
car.forward(0, power);
Delay.msDelay(ms);
leftDistance = sonar.getDistance();
// Get the distance to the right
car.backward(0, power);
Delay.msDelay(ms);
car.forward(power, 0);
Delay.msDelay(ms);
rightDistance = sonar.getDistance();
// Turn in the direction with most space in front of the car
if ( leftDistance > rightDistance ){
car.backward(power, 0);
Delay.msDelay(ms);
car.forward(0, power);
Delay.msDelay(ms);
}
}
}
}
import lejos.nxt.*;
import lejos.util.Delay;
/*
* A simple behavior control program Avoid,
* Figure 9.3 in Jones, Flynn, and Seiger:
* "Mobile Robots, Inspiration to Implementation",
* Second Edition, 1999.
*/
public class AvoidFigure9_3 {
public static void main(String [] args) throws Exception {
PrivateCar car = new PrivateCar();
int power = 70, ms = 500;
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S2);
int frontDistance, leftDistance, rightDistance;
int stopThreshold = 30;
// ESCAPE button listener makes it possible to exit the program
// at any time.
Button.ESCAPE.addButtonListener(new ButtonListener() {
public void buttonPressed(Button b) {
System.exit(1);
}
public void buttonReleased(Button b) {
}
});
LCD.drawString("Avoid 9.3", 0, 0);
Button.waitForAnyPress();
while ( true )
{
// Go forward
car.forward(power, power);
// Monitor the distance in front of the car and stop
// when an object gets to close
frontDistance = sonar.getDistance();
while ( frontDistance > stopThreshold )
{
frontDistance = sonar.getDistance();
}
car.stop();
// Get the distance to the left
car.forward(0, power);
Delay.msDelay(ms);
leftDistance = sonar.getDistance();
// Get the distance to the right
car.backward(0, power);
Delay.msDelay(ms);
car.forward(power, 0);
Delay.msDelay(ms);
rightDistance = sonar.getDistance();
// Drive backwards and spin 180 degrees if everything is too close.
if (leftDistance < stopThreshold && rightDistance < stopThreshold) {
car.forward(0, power);
Delay.msDelay(750);
car.backward(power, power);
Delay.msDelay(ms*3);
car.forward(-power, power);
Delay.msDelay(1750);
continue;
}
// Turn in the direction with most space in front of the car
if ( leftDistance > rightDistance ){
car.backward(power, 0);
Delay.msDelay(ms);
car.forward(0, power);
Delay.msDelay(ms);
}
}
}
}
/**
* A locomotion interface with methods to drive
* a differential car with two independent motors.
*
* @author Ole Caprani
* @version 21.3.14
*/
public interface Car
{
public void stop();
public void forward(int leftPower, int rightPower);
public void backward(int leftPower, int rightPower);
}
public class CarCommand
{
public enum Command {
FORWARD, BACKWARD, STOP
}
public volatile Command command;
public volatile int leftPower, rightPower;
}
import lejos.nxt.*;
/*
* A car driver module with a method to drive
* a differential car with two independent motors. The left motor
* should be connected to port B and the right motor
* to port C.
*
* The car driver method turns car commands into actual commands
* for the two physical motors.
*
* @author Ole Caprani
* @version 21.3.14
*/
public class CarDriver
{
// Commands for the motors
final int forward = 1,
backward = 2,
stop = 3;
private MotorPort leftMotor = MotorPort.B;
private MotorPort rightMotor= MotorPort.C;
public CarDriver()
{
}
private int ccToMc(CarCommand.Command carCommand)
{
switch ( carCommand )
{
case FORWARD: return forward;
case BACKWARD: return backward;
case STOP: return stop;
}
return -1;
}
public void perform(CarCommand command)
{
leftMotor.controlMotor(command.leftPower, ccToMc(command.command));
rightMotor.controlMotor(command.rightPower,ccToMc(command.command));
}
}
/*
* Cruise behavior, p. 303 in
* Jones, Flynn, and Seiger:
* "Mobile Robots, Inspiration to Implementation",
* Second Edition, 1999.
*/
class Cruise extends Thread
{
private SharedCar car;
private int power = 70;
public Cruise(SharedCar car)
{
this.car = car;
}
public void run()
{
while (true)
{
/* Drive forward */
car.forward(power, power);
}
}
}
import java.util.Random;
import lejos.nxt.*;
import lejos.util.Delay;
/*
* Escape behavior
*/
public class Escape extends Thread {
private SharedCar car = new SharedCar();
private int power = 70, ms = 500;
TouchSensor left = new TouchSensor(SensorPort.S1);
TouchSensor right = new TouchSensor(SensorPort.S4);
Random r = new Random();
public Escape(SharedCar car) {
this.car = car;
}
public void run()
{
while (true)
{
// Front bump
if(left.isPressed() && right.isPressed()) {
car.backward(power, power);
Delay.msDelay(ms);
// Either turn right or left (50/50)
if(r.nextBoolean()) {
car.forward(0, power);
Delay.msDelay(ms);
} else {
car.forward(power, 0);
Delay.msDelay(ms);
}
}
// Left bump
else if(left.isPressed()) {
car.backward(power, power);
Delay.msDelay(ms);
car.forward(power, 0);
Delay.msDelay(ms);
}
// Right bump
else if(right.isPressed()) {
car.backward(power, power);
Delay.msDelay(ms);
car.forward(0, power);
Delay.msDelay(ms);
} else {
car.noCommand();
}
}
}
}
import java.util.Random;
import lejos.nxt.*;
import lejos.util.Delay;
/*
* Escape behavior
*/
public class Escape extends Thread {
private SharedCar car = new SharedCar();
private int power = 70, ms = 500;
TouchSensor left = new TouchSensor(SensorPort.S1);
TouchSensor right = new TouchSensor(SensorPort.S4);
Random r = new Random();
public Escape(SharedCar car) {
this.car = car;
}
public void run()
{
while (true)
{
// Front bump
if(left.isPressed() && right.isPressed()) {
car.backward(power, power);
Delay.msDelay(ms);
// Either turn right or left (50/50)
if(r.nextBoolean()) {
car.forward(0, power);
Delay.msDelay(ms);
} else {
car.forward(power, 0);
Delay.msDelay(ms);
}
}
// Left bump
else if(left.isPressed()) {
car.backward(power, power);
Delay.msDelay(ms);
car.forward(power, 0);
Delay.msDelay(ms);
}
// Right bump
else if(right.isPressed()) {
car.backward(power, power);
Delay.msDelay(ms);
car.forward(0, power);
Delay.msDelay(ms);
} else {
car.noCommand();
}
}
}
}
import lejos.nxt.*;
import lejos.util.Delay;
/*
* Follow behavior , inspired by p. 304 in
* Jones, Flynn, and Seiger:
* "Mobile Robots, Inspiration to Implementation",
* Second Edition, 1999.
*/
class Follow extends Thread
{
private SharedCar car = new SharedCar();
private int power = 70, ms = 500;
LightSensor light = new LightSensor(SensorPort.S3);
int frontLight, leftLight, rightLight, delta;
int lightThreshold;
public Follow(SharedCar car)
{
this.car = car;
lightThreshold = light.getLightValue();
}
public void run()
{
while (true)
{
// Monitor the light in front of the car and start to follow
// the light if light level is above the threshold
frontLight = light.getLightValue();
while ( frontLight <= lightThreshold )
{
car.noCommand();
frontLight = light.getLightValue();
}
// Follow light as long as the light level is above the threshold
while ( frontLight > lightThreshold )
{
// Get the light to the left
car.forward(0, power);
Delay.msDelay(ms);
leftLight = light.getLightValue();
// Get the light to the right
car.backward(0, power);
Delay.msDelay(ms);
car.forward(power, 0);
Delay.msDelay(ms);
rightLight = light.getLightValue();
// Turn back to start position
car.backward(power, 0);
Delay.msDelay(ms);
// Follow light for a while
delta = leftLight-rightLight;
car.forward(power-delta, power+delta);
Delay.msDelay(ms);
frontLight = light.getLightValue();
}
car.stop();
Delay.msDelay(ms);
}
}
}
import lejos.nxt.*;
import lejos.util.Delay;
/*
* Follow behavior , inspired by p. 304 in
* Jones, Flynn, and Seiger:
* "Mobile Robots, Inspiration to Implementation",
* Second Edition, 1999.
*/
class NewFollow extends Thread
{
private SharedCar car = new SharedCar();
private int power = 70, ms = 500, rotorPower = 30, rotorMs = 200;
LightSensor light = new LightSensor(SensorPort.S3);
MotorPort rotator = MotorPort.A;
int frontLight, leftLight, rightLight, delta;
int lightThreshold;
public NewFollow(SharedCar car)
{
this.car = car;
lightThreshold = light.getLightValue();
}
public void run()
{
while (true)
{
// Monitor the light in front of the car and start to follow
// the light if light level is above the threshold
frontLight = light.getLightValue();
while ( frontLight <= lightThreshold )
{
car.noCommand();
frontLight = light.getLightValue();
}
// Follow light as long as the light level is above the threshold
while ( frontLight > lightThreshold )
{
// Stop the car while reading values.
car.stop();
// Get the light to the left
rotator.controlMotor(power, MotorPort.FORWARD);
Delay.msDelay(rotorMs);
leftLight = light.getLightValue();
// Get the light to the right
rotator.controlMotor(power, MotorPort.BACKWARD);
Delay.msDelay(rotorMs*2);
rightLight = light.getLightValue();
// Turn back to start position
rotator.controlMotor(power, MotorPort.FORWARD);
Delay.msDelay(rotorMs);
rotator.controlMotor(0, MotorPort.STOP);
// Follow light for a while
delta = leftLight-rightLight;
car.forward(power-delta, power+delta);
Delay.msDelay(ms);
frontLight = light.getLightValue();
}
car.stop();
Delay.msDelay(ms);
}
}
}
import lejos.nxt.*;
/**
* A locomotion module with methods to drive
* a differential car with two independent motors. The left motor
* should be connected to port B and the right motor
* to port C.
*
* @author Ole Caprani
* @version 21.3.14
*/
public class PrivateCar implements Car
{
// Commands for the motors
private final int forward = 1,
backward = 2,
stop = 3;
private MotorPort leftMotor = MotorPort.B;
private MotorPort rightMotor= MotorPort.C;
public PrivateCar()
{
}
public void stop()
{
leftMotor.controlMotor(0,stop);
rightMotor.controlMotor(0,stop);
}
public void forward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,forward);
rightMotor.controlMotor(rightPower,forward);
}
public void backward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,backward);
rightMotor.controlMotor(rightPower,backward);
}
}
import lejos.nxt.*;
/*
* Behavior control network of Figure 9.9 in chapter 9 in
* Jones, Flynn, and Seiger:
* "Mobile Robots, Inspiration to Implementation",
* Second Edition, 1999.
*/
public class RobotFigure9_9 {