Código de la práctica 2

practica2.java
/***************************************************************************/
/* Nombre: Practica2
*/
/* Autores: Daniel Montero Martín /// Alberto Sánchez González
*/
/* Fecha:
*/
/***************************************************************************/
import lejos.nxt.*;
import lejos.navigation.Pilot;
public class practica2 {
static
static
static
static
static
static
static
static
static
static
static
static
static
static
float DiametroRuedas = 5.6f;
float DistanciaEntreRuedas = 11.6f;
Pilot pilot = new Pilot(DiametroRuedas,DistanciaEntreRuedas,Motor.A, Motor.C);
Motor brazo = new Motor (MotorPort.B);
RCXLightSensor lsAzul = new RCXLightSensor(SensorPort.S1 );//Sensor Azul
UltrasonicSensor us = new UltrasonicSensor( SensorPort.S2);
LightSensor lsBlanco = new LightSensor(SensorPort.S3,true);//Sensor Blanco
int Vgiro=200;//velocidad de giro del robot y del brazo al subir
int Vtravel=300;//velocidad del robot
int Vacecho=100;//velocidad cuando la pelota está muy cerca
int Vbrazo=400;//velocidad del brazo al bajar
int nobola;//Variable calibracion sensor azul.
int Gbuscar=60;//angulo al encontrar linea negra en la funcion buscar pelota
int Grosa=110;//angulo al encontrar linea negra en la funcion buscar rosa
/*
* dist: distancia que avanza
* ang: angulo que gira
* Avanza la distancia y luego rota el angulo.
*/
public static void cuandoNegro(int dist,int ang){
pilot.stop();
pilot.setSpeed(Vtravel);
pilot.travel(dist);
rotar(ang);
}
/*
* Funciones para comprobar el color del suelo: blanco, negro y el basurero.
* RETURN BOOLEAN TRUE si es ese color.
* RETURN BOOLEAN FALSE en caso contrario.
*/
public static boolean compruebaBlanco(int value){
return (value>85);
}
public static boolean compruebaNegro(int value){
return (value<15);
}
public static boolean compruebaBasurero(int value){
return ((value>35)&&(value<85));
}
/*
* Avanza mientras vea blanco hasta una linea negra o un objeto a menos de 40.
* RETURN FALSE si objeto.
* RETURN TRUE si no es blanco.
*/
-1-
practica2.java
public static boolean viajaHastaNegro(){
pilot.setSpeed(Vtravel);
pilot.forward();
while (compruebaBlanco(lsBlanco.readValue()) && (us.getDistance() > 40)){
}
pilot.stop();
return(!compruebaBlanco(lsBlanco.readValue()));
}
/*
* angulo: angulo que gira
* Rota el angulo.
*/
public static void rotar(int angulo){
pilot.stop();
pilot.setSpeed(Vgiro);
pilot.rotate(angulo);
pilot.setSpeed(Vtravel);
}
/*
* Avanza mientras es blanco.
* Cuando no es blanco, comprueba negro.
* Si es negro se va, si no lo es avanza 2 y comprueba negro.
* Si es negro se va, si no comprueba basurero.
* Si es basurero retrocede 2 y suelta la pelota, si no se va.
*/
public static void buscarRosa(){
boolean pelotaDejada=false;
while (!pelotaDejada){
pilot.setSpeed(Vgiro);
pilot.forward();
while (compruebaBlanco(lsBlanco.readValue())){
}
pilot.stop();
if (compruebaNegro(lsBlanco.readValue())){
pilot.stop();
pilot.setSpeed(Vgiro);
pilot.travel(-20);
//brazo.rotate(-5);
rotar(Grosa);
}else{
pilot.travel(2);
if (compruebaNegro(lsBlanco.readValue())){
pilot.stop();
pilot.setSpeed(Vgiro);
pilot.travel(-22);
//brazo.rotate(-5);
rotar(Grosa);
}else{
if (compruebaBasurero(lsBlanco.readValue())){
pilot.travel(-2);
brazo.setSpeed(Vgiro);
brazo.rotate(180);
brazo.setSpeed(Vbrazo);
pilot.travel(-20);
rotar(180);
-2-
practica2.java
pelotaDejada=true;
}else{
pilot.stop();
pilot.setSpeed(Vgiro);
pilot.travel(-22);
//brazo.rotate(-5);
rotar(Grosa);
}
}
}
}
}
/*
* Calibrar sensor blanco, alto para blanco y bajo para negro.
*/
public static void calibrarBlanco(){
int opcion = 0;
String[] viewItems = {"-Blanco", "-Negro", "-OK"};
TextMenu main = new TextMenu(viewItems, 1, "Calibrar Sensor");
while(opcion != 2)
{
LCD.clear();
opcion = main.select();
if (opcion == 0) //Blanco
{
while (Button.ENTER.isPressed()){
LCD.clear();
LCD.drawString("Calibrando Blanco", 0, 0);
LCD.drawString("Value:", 0, 2);
lsBlanco.calibrateHigh();
LCD.drawInt(lsBlanco.readNormalizedValue(), 9, 2);
LCD.refresh();
try {
Thread.sleep (1000);
} catch (InterruptedException e) {
LCD.drawString(")", 13, 2);;
}
}
}
if (opcion == 1) //Negro
{
while (Button.ENTER.isPressed()){
LCD.clear();
LCD.drawString("Calibrando Negro", 0, 0);
LCD.drawString("Value:", 0, 2);
lsBlanco.calibrateLow();
LCD.drawInt(lsBlanco.readNormalizedValue(), 9, 2);
LCD.refresh();
try {
Thread.sleep (1000);
} catch (InterruptedException e) {
LCD.drawString(")", 13, 2);;
}
}
-3-
practica2.java
}
}
}
/*
* Calibrar sensor azul para cuando no detecta pelota.
*/
public static void calibrarAzul(){
nobola = lsAzul.readValue();
}
/*
* Inicializa el brazo subiendolo.
*/
public static void inicializarBrazo(){
brazo.setSpeed(Vbrazo);
brazo.rotate(180);
brazo.setSpeed(Vgiro);
}
/*
* Mientras no ha encontrado la pelota sigue buscandola.
* Si la localiza a <80 y >50 se acerca 20 cm de 2 en 2.
* Si la localiza a <50 se centra 1 vez mirando hacia la pelota.
* Si la localiza a <50 y >30 se acerca 20 cm de 2 en 2.
* Si la localiza a <30 avanza hasta que el sensor azul reconoce la pelota.
*/
public static void buscar(){
boolean localizado;
boolean encontrado = false;
int angulo,giro;
int tope;
int aux;
pilot.stop();
while (!encontrado){
localizado = false;
angulo = 0;
giro = 15;
while ((!localizado) && (angulo<361)){
pilot.setSpeed(Vtravel);
tope = 0;
if (us.getDistance() < 80){ //no tiene else
localizado = true;
if (us.getDistance() > 50){
aux = 0;
while(compruebaBlanco(lsBlanco.readValue()) && (aux<20)){
pilot.travel(3);
aux=aux+3;
}
if (compruebaBlanco(lsBlanco.readValue())){
rotar(-15);
angulo = angulo - 15;
}else{
cuandoNegro(-20,Gbuscar);
viajaHastaNegro();
-4-
practica2.java
}
}else{
angulo = angulo - 15;
rotar(-18);
while (us.getDistance() >=100){
angulo = angulo +5;
rotar(5);
}
while (us.getDistance() < 70){
tope = tope + 5;
angulo = angulo + 5;
rotar(5);
}
tope=tope+5;
rotar((tope/2)*(-1));
if (us.getDistance() > 30){
aux = 0;
while(compruebaBlanco(lsBlanco.readValue()) && (aux<20)){
pilot.travel(3);
aux=aux+3;
}
if (compruebaBlanco(lsBlanco.readValue())){
rotar(-15);
angulo = angulo - 15;
}else{
cuandoNegro(-20,Gbuscar);
viajaHastaNegro();
}
}else{
pilot.setSpeed(Vacecho);
while((lsAzul.readValue()< (nobola+5))&&(compruebaBlanco(lsBlanco
.readValue()))){
pilot.travel(5);
}
if (compruebaBlanco(lsBlanco.readValue())){
pilot.stop();
brazo.setSpeed(Vtravel);
brazo.rotate(-180);
encontrado = true;
}else{
cuandoNegro(-20,Gbuscar);
viajaHastaNegro();
}
}
}
giro = 0;
}//us.getDistance() < 80
pilot.setSpeed(Vgiro);
rotar(giro);
angulo = angulo + 15;//25 giros
}
if (!localizado){
if(viajaHastaNegro()){
cuandoNegro(-20,Gbuscar);
if(viajaHastaNegro()){
cuandoNegro(-10,Gbuscar);
}
-5-
practica2.java
}
}
}
}
/////////////////////////////////////////
//////////
MAIN
//////////////
////////////////////////////////////////
public static void main (String[] aArg){
inicializarBrazo();
calibrarAzul();
calibrarBlanco();
while(!Button.ESCAPE.isPressed()){
buscar();
buscarRosa();
}
}
}
-6-