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(), p.getY());
- min = Math.min(min, d);
- }
- System.out.println(Math.sqrt(min));
- return Math.sqrt(min);
- }
- }
|