1234567891011121314151617181920212223242526 |
- 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) {
- // o modelo apresentado aqui eh o mais burro
- // ele pega a menor distancia do robo a um segmento
- double min = Double.MAX_VALUE;
- double d;
- for (Line l : map.getLines()) {
- d = l.ptSegDist(p.getX() * 10, p.getY() * 10);
- min = Math.min(min, d);
- }
- //System.out.println(min);
- return min / 10.0;
- }
- }
|