Package lejos.robotics.proposal

Source Code of lejos.robotics.proposal.ArcPoseController

package lejos.robotics.proposal;

import lejos.geom.Point;
import lejos.robotics.Pose;
import lejos.robotics.localization.PoseProvider;

/*
* WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
* DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
*/

/**
* This class directs a pilot from the current known coordinates to a destination set of coordinates.
* The ArcPoseController is not capable of avoiding objects or planning a route. It can only drive in a straight
* line that is not obstructed. 
*
*/
public class ArcPoseController implements PoseController {
  private ArcPilot pilot;
  private PoseProvider poseProvider;
 
  public ArcPoseController(ArcPilot pilot, PoseProvider poseProvider) {
    this.pilot = pilot;
    this.poseProvider = poseProvider;
  }
 
  public ArcPilot getPilot() {
    return pilot;
  }

  public PoseProvider getPoseProvider() {
    return poseProvider;
  }

  public void setPoseProvider(PoseProvider replacement) {
    poseProvider = replacement;
  }

  public Pose goTo(Point destination) {
    Pose pose = poseProvider.getPose();
    if (pilot instanceof RotatePilot) { // optimize for RotatePilot
        float turnAngle = pose.angleTo(destination) - pose.getHeading();
        while (turnAngle < -180)turnAngle += 360;
        while (turnAngle > 180) turnAngle -= 360;
      ((RotatePilot) pilot).rotate(turnAngle);
      pilot.travel(pose.distanceTo(destination));     
    } else {
      //TODO: Calculate arc of minimum radius needed to point to the
      // destination and the do an arc and a travel.
    }
    return poseProvider.getPose();
  }

  public Pose goTo(float x, float y) {
    return goTo(new Point(x, y));
  }
}
TOP

Related Classes of lejos.robotics.proposal.ArcPoseController

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.