Programar el robot para que utilizando el sensor de toque pueda encontrar y rodear un
objeto siguiendo su contorno. El objeto puede tener cualquier forma (rectangular como una caja o cilíndrica como un bote, el objeto será suficientemente pesado como para que el robot no lo mueva, dentro de límites razonables. El objeto podrá estar en cualquier posición con respecto del robot dentro de un área de 15 m².
import lejos.nxt.*;import lejos.nxt.comm.RConsole;import lejos.robotics.navigation.*;import lejos.robotics.subsumption.*;
public class GoAround{
public static void main(String[] args){ TachoPilot pilot = new TachoPilot(5.6f, 11.25f, Motor.B, Motor.C); TouchSensor touch = new TouchSensor(SensorPort.S3); UltrasonicSensor ultrasonic = new UltrasonicSensor(SensorPort.S1);
Behavior goForward = new GoForward(pilot); Behavior hitObject = new HitObject(pilot, touch, ultrasonic);
Behavior [] behaviorArray = {goForward, hitObject}; Arbitrator arbitrator = new Arbitrator(behaviorArray);
LCD.drawString("GoAround", 2, 2); Button.waitForPress(); arbitrator.start(); }
}import lejos.nxt.LCD;import lejos.robotics.subsumption.*;import lejos.robotics.navigation.*;
public class GoForward implements Behavior{
private Pilot pilot;
public GoForward(Pilot pilot){ this.pilot = pilot; }
public boolean takeControl(){ LCD.clear(); LCD.drawString("GoForward", 2, 2); return true; }
public void suppress(){ pilot.stop(); }
public void action(){ pilot.forward(); }
}
import lejos.nxt.*;import lejos.robotics.subsumption.*;import lejos.robotics.navigation.*;
public class HitObject implements Behavior, SensorPortListener{ private TachoPilot pilot; private TouchSensor touch; private UltrasonicSensor ultrasonic; private boolean hit;
private int backoff; private int rotationAngle; private int distance; private int lastDistance; private int distanceDifference;
private int veryCloseLeft; private int slightlyCloseLeft; private int notCloseLeft; private int slightlyFarRight; private int veryFarRight;
private int basePowerLeft; private int basePowerRight;
private Motor rightMotor; private Motor leftMotor;
public HitObject(TachoPilot pilot, TouchSensor touch, UltrasonicSensor ultrasonic){ this.pilot = pilot; this.touch = touch; this.ultrasonic = ultrasonic;
hit = false; SensorPort.S3.addSensorPortListener(this);
backoff = 5; rotationAngle = -90; distance = 0; lastDistance = 0;
veryCloseLeft = 20; slightlyCloseLeft = 25; notCloseLeft = 30;
slightlyFarRight = 35; veryFarRight = 40;
basePowerLeft = 100; basePowerRight = 99;
rightMotor = (Motor)pilot.getRight(); leftMotor = (Motor)pilot.getLeft(); }
public void stateChanged(SensorPort port, int oldValue, int newValue){ if(touch.isPressed()){ hit = true; } }
public boolean takeControl(){ if(hit){ LCD.clear(); LCD.drawString("HitObject", 2, 2); hit = false; return true; } else{ return false; } }
public void suppress(){ pilot.stop(); }
public void action(){ pilot.stop(); pilot.travel(-backoff); pilot.rotate(rotationAngle);
while(true){ lastDistance = distance; distance = ultrasonic.getDistance(); distanceDifference = lastDistance - distance;
if(distance <= notCloseLeft){ if(distance <= veryCloseLeft){ rightMotor.setPower(basePowerRight); rightMotor.backward(); leftMotor.setPower(basePowerRight); leftMotor.forward(); } else if(distance <= slightlyCloseLeft){ rightMotor.setPower(0); rightMotor.stop(); leftMotor.setPower(basePowerLeft); leftMotor.forward(); } else if(distance <= notCloseLeft){ leftMotor.setPower(basePowerLeft); leftMotor.forward(); rightMotor.setPower(0); rightMotor.flt(); } } else{ if(distance >= slightlyFarRight){ if(distanceDifference < 2){ leftMotor.setPower(0); leftMotor.stop(); rightMotor.setPower(basePowerRight); rightMotor.forward(); } } else if(distance >= veryFarRight){ leftMotor.setPower(basePowerLeft/2); leftMotor.forward(); rightMotor.setPower(basePowerLeft); rightMotor.forward(); } }
try { Thread.sleep(30); } catch(InterruptedException e){
}
}
}
}