Package lejos.robotics

Examples of lejos.robotics.Pose.distanceTo()

52
53
54
55
56
57
58
59
60
61
62
    Pose pose = start;
   
    // Continue until we return a route or throw DestinationUnReachableException
    for(;;) {
      // If the current pose if close enough to the destination, go straight there
      if (pose.distanceTo(destination.getLocation()) < MAX_DISTANCE) {
        add(new WayPoint(destination));
        return this;
      } else {
        Pose testPose = null;
       
View Full Code Here
63
64
65
66
67
68
69
70
71
72
73
        // Generate random poses and apply tests to them
        for(int i=0;i<MAX_ITERATIONS;i++) {
            testPose = generatePose();
           
            // The new Pose must not be more than MAX_DISTANCE away from current pose     
            if (testPose.distanceTo(pose.getLocation()) > MAX_DISTANCE) continue;
           
          // The new pose must be at least MIN_GAIN closer to the destination
          if (pose.distanceTo(destination.getLocation()) -
              testPose.distanceTo(destination.getLocation()) < MIN_GAIN)
            continue;
View Full Code Here
67
68
69
70
71
72
73
74
75
76
77
            // The new Pose must not be more than MAX_DISTANCE away from current pose     
            if (testPose.distanceTo(pose.getLocation()) > MAX_DISTANCE) continue;
           
          // The new pose must be at least MIN_GAIN closer to the destination
          if (pose.distanceTo(destination.getLocation()) -
              testPose.distanceTo(destination.getLocation()) < MIN_GAIN)
            continue;
         
          // We must be able to get a valid set of range readings from the new pose
          float heading = testPose.getHeading();
          boolean validReadings = true;
View Full Code Here
85
86
87
88
89
90
91
92
93
94
95
          }         
          if (!validReadings) continue;
         
          //Check there are no obstacles in the way
          testPose.setHeading(testPose.angleTo(pose.getLocation()));
          if (map.range(testPose) < testPose.distanceTo(pose.getLocation()))
            continue;         
         
          testPose.setHeading(heading); // Restore heading
          break; // We have a new way point
        }
View Full Code Here
41
42
43
44
45
46
47
48
49
50
51
    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();
View Full Code Here
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.