Commit 4896a6b8 authored by Enniwhere's avatar Enniwhere
Browse files

Finished the light following robot with escape behavior and a light sensor motor.

parent ee0b1159
Pipeline #3181 skipped
......@@ -16,8 +16,8 @@ build.dir=build
classes.dir=${build.dir}/classes
docs.dir=${build.dir}/docs
main.class=AvoidFigure9_3_Upg
output.basename=
main.class=RobotFigure9_9_Fol
output.basename=9_9_EscFol
main.pc_class=
output.pc_basename=
......
......@@ -10,8 +10,8 @@ class Escape extends Thread
private SharedCar car = new SharedCar();
private int power = 70, ms = 500, backwards_time = 2000;
TouchSensor leftBumper = new TouchSensor(SensorPort.S2);
TouchSensor rightBumper = new TouchSensor(SensorPort.S3);
TouchSensor leftBumper = new TouchSensor(SensorPort.S3);
TouchSensor rightBumper = new TouchSensor(SensorPort.S2);
int frontDistance, leftDistance, rightDistance;
int stopThreshold = 30;
......
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.util.Delay;
/*
* Follow behavior , inspired by p. 304 in
* Jones, Flynn, and Seiger:
* "Mobile Robots, Inspiration to Implementation",
* Second Edition, 1999.
*/
class FollowSensorMotor extends Thread
{
private SharedCar car = new SharedCar();
private int power = 70, ms = 500;
LightSensor light = new LightSensor(SensorPort.S4);
int frontLight, leftLight, rightLight, delta;
int lightThreshold;
int rightAngle = 45, leftAngle = -45;
public FollowSensorMotor(SharedCar car)
{
this.car = car;
lightThreshold = light.getLightValue();
Motor.A.resetTachoCount();
}
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 )
{
car.stop();
// Rotate to the left angle and get the left light
Motor.A.rotateTo(leftAngle);
while(Motor.A.isMoving()){yield();}
leftLight = light.getLightValue();
// Get the light to the right
Motor.A.rotateTo(rightAngle);
while(Motor.A.isMoving()){yield();}
rightLight = light.getLightValue();
// Turn back to start position
Motor.A.rotateTo(0);
while(Motor.A.isMoving()){yield();}
// 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);
}
}
}
......@@ -29,6 +29,8 @@ public class RobotFigure9_9_Esc {
arbiter.setDaemon(true);
arbiter.start();
escape.setDaemon(true);
escape.start();
avoid.setDaemon(true);
avoid.start();
follow.setDaemon(true);
......
import lejos.nxt.Button;
import lejos.nxt.LCD;
/*
* 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_Fol {
public static void main(String [] args) throws Exception {
SharedCar [] car = { new SharedCar(), new SharedCar(),
new SharedCar(), new SharedCar()};
CarDriver cd = new CarDriver();
Escape escape = new Escape(car[0]);
Avoid avoid = new Avoid(car[1]);
FollowSensorMotor follow = new FollowSensorMotor(car[2]);
Cruise cruise = new Cruise(car[3]);
Arbiter arbiter = new Arbiter(car, cd);
LCD.drawString("Robot 9.9 Esc", 0, 0);
Button.waitForAnyPress();
arbiter.setDaemon(true);
arbiter.start();
escape.setDaemon(true);
escape.start();
avoid.setDaemon(true);
avoid.start();
follow.setDaemon(true);
follow.start();
cruise.setDaemon(true);
cruise.start();
while ( ! Button.ESCAPE.isDown())
{
LCD.drawString("Winner " + arbiter.winner(), 0, 3);
}
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment