Práctica 1 { parte 6: Visualización de la trayectoria}

Haremos un programa con el cual mostremos en el LCD la trayectoria en tiempo real que va siguiendo el robot.
Para ello haremos uso de la clase TachoPilot y LCD.

En el LCD iremos mostrando posición X, posición Y, y ángulo. Comenzaremos en (0,0,0) e iremos actualizando la posición y grado según esta fórmula:

Aquí vemos un vídeo de como nos lo muestra en el LCD del robot:




Como se observa en el vídeo, en el LCD se muestra que llega hasta 80cm, cuando se debería hacer hasta 40. Hemos hecho una traza y vemos que realmente se mueve 40cm, pero en la posición muestra 80cm.

Aquí podemos ver el código:

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.*;

public class Trayectoria {

public static void main(String[] args) throws InterruptedException {

Motor Rizda = Motor.B;

Motor Rdcha = Motor.C;

int i;

int x=0;

int y=0;

int dist=0;

int grados=0;

TachoPilot viajar = new TachoPilot(5.6f,10.85f,Rizda,Rdcha);

for(i=1;i<=4;i++){

viajar.reset();

viajar.travel(40,true);

while (viajar.getTravelDistance() < 39.9){

dist=(int) viajar.getTravelDistance();

x=(int) (x+dist*Math.cos(Math.toRadians(grados)));

y=(int) (y+dist*Math.sin(Math.toRadians(grados)));

LCD.clear();

LCD.drawInt(x,7,3);

LCD.drawInt(y,7,4);

LCD.drawInt(grados,7,5);

Thread.sleep(500);

}

viajar.rotate(90);

grados=(int) (grados + viajar.getAngle());

LCD.clear();

LCD.drawInt(x,7,3);

LCD.drawInt(y,7,4);

LCD.drawInt(grados,7,5);

Thread.sleep(250);

}

}

}