| 123456789101112131415161718192021222324 |
- package config;
- 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 = new Pose(p.getX(), p.getY(), p.getHeading());
- float mindist = Float.POSITIVE_INFINITY;
- int cone = 30;
- for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
- tmppose.setHeading((float) (p.getHeading() - angulo + angle));
- float dist = map.range(tmppose);
- if (dist > 0 && dist < mindist) mindist = dist;
- }
- return mindist;
- }
- }
|
