Homer.java

import java.awt.Rectangle;
import java.io.PrintStream;
import java.util.Collection;
import lejos.geom.Line;
import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.Pose;
import lejos.robotics.RangeReadings;
import lejos.robotics.RangeScanner;
import lejos.robotics.localization.MCLParticleSet;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.mapping.RangeMap;
import lejos.robotics.proposal.ArcPoseController;
import lejos.robotics.proposal.DestinationUnreachableException;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.robotics.localization.MCLPoseProvider;
import lejos.robotics.proposal.MapPathFinder;
import lejos.robotics.proposal.PathFinder;
import lejos.robotics.proposal.PoseController;
import lejos.robotics.proposal.WayPoint;

/**
 * Test of Monte Carlo Localisation, Pose Controllers and Path finders.
 * 
 * The robot is put down in a random position in the mapped area. 
 * It makes random moves until if works out where it is. 
 * It then uses a path finder to find a route home. 
 * A pose controller is used to follow the route.
 * 
 * The robot is an robot that is supported by the DifferentialPilot class.
 * A range scanner located above the robot's center of rotation is required.
 * 
 * @author Lawrie Griffiths
 *
 */
public class Homer implements RangeScanner {
  // Tyre diameter and wheel base
  private static final float TYRE_DIAMETER = 5.6f;
  private static final float WHEEL_BASE = 16.0f;

  private static int BORDER = 10; 
  private static int NUM_PARTICLES = 200;
  private static int MAX_RELIABLE_RANGE_READING = 150;
  private static float RANGE_READING_ANGLE = 45;
  private static int FORWARD_READING = 1;
  private static int MAX_DISTANCE = 40;
  
  // Distance from ultrasonic sensor to front of robot in cm
  private static final float PROJECTION = 10.0f;
  
  private static UltrasonicSensor range = new UltrasonicSensor(SensorPort.S1);
  private static RangeReadings readings = new RangeReadings(3);
  
  // Array of lines for the map
  private static final Line[] lines = { 
        new Line(32, 0, 32, 88), new Line(32, 88, 0, 88), 
        new Line(0, 88, 0, 340), new Line(0, 340, 95, 340), 
        new Line(95, 340, 95, 294), new Line(95, 294, 132, 294), 
        new Line(132, 294, 132, 0), new Line(132, 0, 32, 0)};
  
  private static final Rectangle bound = new Rectangle(0,0,132,340); 
  private static RangeMap map = new LineMap(lines, bound);
  private static DifferentialPilot pilot;
  private static MCLParticleSet particles;
  private static MCLPoseProvider mcl;
  
  public static void main(String[] args) {
    Homer simpson = new Homer();
    simpson.run();
  }
  
  public void run() {  
    //RConsole.openBluetooth(0);
    //System.setOut(new PrintStream(RConsole.openOutputStream()));
    
    // Create the robot and MCL pose provider and get its particle set
    pilot = new DifferentialPilot( 
        TYRE_DIAMETER, WHEEL_BASE, Motor.B, Motor.C, true);
    mcl = new MCLPoseProvider(pilot,this, map, NUM_PARTICLES, BORDER);
    particles = mcl.getParticles();
    particles.setDebug(true);
    
    // Make random moves until we know where we are
    Pose start = localize(); 
  
    // Find a route home
    Pose home = new Pose(50, 300, -90);
    PathFinder pf = new MapPathFinder(map, readings);
    PoseController pc = new  ArcPoseController(pilot, mcl);
    
    System.out.println("Located: (" + (int) start.getX() + "," + (int) start.getY() + ")");
    
    // Go home
    try {
      Collection<WayPoint> route = pf.findRoute(start, home);
      
      for(WayPoint wp: route) {
        System.out.println("Go to (" + (int) wp.x + "," + (int) wp.y + ")");
        Pose pose = pc.goTo(wp);
        goodEstimate(pose); // Just for diagnostics
        // Pose controller should have a goTo(Pose) method to do this
        pilot.rotate(wp.getHeading() - pose.getHeading());
      }
    } catch (DestinationUnreachableException e) {
      System.out.println("Unreachable");
    }
    Button.waitForPress();
  }

  @Override
  public RangeReadings getRangeValues() {
    takeReadings();
    return readings;
  }
  
  /**
   * Take a single range reading
   * 
   */
  private void takeReading(float angle, int i) {
    int rangeByte = (int) range.getRange();
    float range;

    if (rangeByte > MAX_RELIABLE_RANGE_READING) range = -1f;
    else range = ((float) rangeByte);
    readings.setRange(i,angle, range);
  }
  
  /**
   * Take a set of 3 readings
   */
  public void takeReadings() {
    // Take forward reading
    takeReading(0f, FORWARD_READING);

    // Take left reading
    pilot.rotate(-RANGE_READING_ANGLE);
    takeReading(-RANGE_READING_ANGLE, 0);

    // Take right reading
    pilot.rotate(2 * RANGE_READING_ANGLE);
    takeReading(RANGE_READING_ANGLE, 2);
    pilot.rotate(-RANGE_READING_ANGLE);
  }
  
  /**
   * Make a random move
   */
  public void randomMove() {
    float angle = (float) Math.random() * 360;
    float distance = (float) Math.random() * MAX_DISTANCE;
    
    if (angle > 180f) angle -= 360f;

    // Get forward range
    float forwardRange = readings.getRange(1);

    // Don't move forward if we are near a wall
    if (forwardRange < 0
        || distance + BORDER + PROJECTION < forwardRange)
      pilot.travel(distance);
    
    pilot.rotate(angle);
  }
  
  /**
   * Check if estimated pose is accurate enough
   */
  boolean goodEstimate(Pose pose) {
    int width = particles.getErrorRect().width;
    int height = particles.getErrorRect().height;
    System.out.println("At " + (int) pose.getX() + "," + (int)  pose.getY() + " Error: " + width + "," + height + " Weight: " + particles.getMaxWeight());
    return width < 50 && height < 50;
  }
  
  /**
   * Make random moves until the estimated pose is good enough
   */
  private Pose localize() {
    for(;;) {
      Pose pose = mcl.getPose();
      if (goodEstimate(pose)) return pose;
      else randomMove();
    }
  }
}