| 123456789101112131415161718192021222324 |
- package config;
- import lejos.geom.Line;
- import lejos.robotics.mapping.LineMap;
- import lejos.robotics.navigation.Pose;
- public class Models {
- private LineMap map;
- public Models(LineMap map) {
- this.map = map;
- }
- public double expectedSonarRead(Pose p, double angle) {
- Pose tmppose = (30, 25, 120);
- float mindist = Float.POSITIVE_INFINITY;
- for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
- tmppose.setHeading(mypose.getHeading() - angulo);
- float dist = mymap.range(tmpPose);
- if (dist > 0 && dist < mindist) mindist = dist;
- }
- return 0;
- }
- }
|
